EXPERIMENTS  IN  HIGH  ^ERFORf/lANCE  CONTROL 

OF  A  MULTI  LINK  I 
M  I-TI  MANIPULATOR 


4.  AUTHCRtS) 

William  L.  Ballhaus 

Prof.  Stephen  M.  Rock,  Principal  Advisor 


7.  MKKIIMfiM  OAGAMUAHOM  MAMf(S)  AMO  AOOAISS<£S) 


Aerospace  Robotics  Laboratory 
Department  of  Aeronautics  and  Astronautics 
Stanford  University 
Stanford,  CA  94305 


5.  MUMAftJU 

F496  20-93- 1-02  48-?000(l 


f.  S^CMSOMM  MOMITOMMC  AGiMO  MAM((S)  AMO  AOOMiSS^SS 


AFOSR/NA 
Bolling  AFB 
Washington,  DC 


•-  ^(VOItMWG  OnOAMOAno^ 

npom  NuMsiA 

SUDAAR  649 


AFOSR-TR.96 


1 


20332-6448 


Ua.  OtSTMUnOM/ AVA&AMJTY  STATIMINT 


Approved  for  public  release, 
distribution  unlimited 


UM.  OtSTMunOM  COM 


13-  AASTRACr  (Msjomum  200  wormi 
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long  slender  links  that  are  inherently  very  flexible,  which  flexibility  slows  and 
degrades  end-point  position  performance  and  makes  high-performance  control  difficult. 
This  project’s  research  focuses  on  increasing  the  achievable  slew  and  end-point 
performance  of  multi-link  flexible  manipulators  through  use  of  a  small,  high-bandwidti 
rigid  robot  at  the  tip,  together  with  direct  end-point  sensing.  A  new  soft— terminal 
control  approach  has  been  developed  for  multi-link  flexible  manipulators  that 
exploits  both  high-bandwidth  local-manipulation  capability  and  the  redundancy  intro¬ 
duced  by  the  mani-manipulator ,  and  the  fact  that  the  flexible  main  arm  can  be 
positioned  more  guickly  to  be  within  an  area  than  it  can  be  position  to  be  at  a 
point.  In  developing  this  new  control  approach,  fundamental  advances  were  made  in 
trajectory  generation,  feedback-control  design,  and  modelling  of  multi-link  flexible 
manipulators . 
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Abstract 


Space-based  manipulator  systems  like  the  Shuttle  Remote  Manipulator  System  contain 
long  slender  links  that  are  inherently  very  flexible.  As  a  result,  when  moving  quickly  or 
carrying  massive  payloads,  the  links  of  these  manipulators  deflect  signiflcantly.  This  link 
flexibility  slows  and  degrades  end-point  position  performance  and  makes  high-performance 
control  of  these  manipulators  difficult  to  achieve. 

The  research  presented  in  this  dissertation  focuses  on  increasing  the  achievable  slew  and 
end-point  performance  of  multi-link  flexible  manipulators  through  the  use  of  a  small,  high- 
bandwidth,  rigid  robot  (mini-manipulator)  at  the  tip,  together  with  direct  end-point  sensing. 
The  high-bandwidth  mini-manipulator  can  be  used  for  rapid,  precise  control  within  a  small 
workspace,  while  the  main  manipulator  is  responsible  for  transporting  the  mini-manipulator 
from  workspace  to  workspace. 

The  overall  system  performance  of  a  multi-link  flexible  manipulator  with  a  mini-manip¬ 
ulator  is  limited,  in  part,  by  the  ability  of  the  flexible  main  arm  to  position  quickly  the 
mini-manipulator  within  the  desired  workspace.  This  is  mitigated,  however,  by  the  fact 
that  even  for  very  high-performance  control,  the  flexible  main  arm  need  be  controlled  only 
to  an  area,  rather  than  to  a  point;  an  area  within  which  the  speed  and  precision  of  the  mini¬ 
manipulator  can  then  be  utilized  to  perform  the  task  at  hand.  This  new  generic  concept 
of  controlling  to  an  area  is  pursued  in  this  research  through  a  new  technique  known  as  soft 
terminal  control 

A  new  soft-terminal-control  approach  has  been  developed  for  multi-link  flexible  ma- 
nipulators  that  exploits  both  the  high-bandwidth  local-manipulation  capability  and  the 
redundancy  introduced  by  the  mini-manipulator,  and  the  fact  that  the  flexible  main  arm 
can  be  positioned  more  quickly  to  be  within  an  area  than  it  can  be  positioned  to  be  at 
a  point.  This  approach,  based  on  a  terminal-controller  design,  combines  feedforward  with 
feedback  control.  In  developing  this  new  control  approach,  fundamental  advances  were 
made  in  trajectory  generation,  feedback-control  design,  and  modelling  of  multi-link  flexible 
manipulators. 

To  aid  in  the  development  and  verification  of  this  control  approach,  an  experimental  test 
bed  has  been  developed.  This  apparatus  consists  of  a  two-link  very  flexible  manipulator 
with  a  mini-manipulator  mounted  at  its  tip.  This  system  floats  on  an  air  bearing  to  simulate 
in  two  dimensions  the  zero-g,  frictionless  environment  of  space. 
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This  dissertation  contains  a  description  of  the  theoretical  and  experimental  advances 
made  in  trajectory  generation,  feedback-control  design,  and  modelling  of  multi-link  flexible 
manipulators.  Also  included  are  experimental  results  that  demonstrate,  for  the  first  time, 
the  performance  advantages  of  a  mini-manipulator  and  direct  end-point  sensing  at  the  tip 
of  a  multi-link  flexible  manipulator,  and  of  their  system  operation  as  a  redundant  team. 
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Chapter  1 


Introduction 


This  dissertation  presents  theoretical  and  experimental  research  on  the  high-performance 
control  of  generic  multi-link  flexible  space  manipulator  systems,  and  specifically  of  a  two- 
link  flexible  manipulator  with  a  mini-manipulator  at  its  tip.  This  research  was  conducted 
at  the  Stanford  University  Aerospace  Robotics  Laboratory  (ARL)  at  Stanford  University 
from  1991  to  1994. 


1.1  Motivation 

1.1.1  Robotics 

Robots  are  well  suited  for  many  tasks  performed  currently  by  humans.  Examples  include 
high-speed  pick-and-place  maneuvers,  automated  inspection,  assembly,  and  manufacturing 
tasks.  Robots  are  especially  well  suited  to  operate  in  harsh  environments,  such  as  space  or 
hazardous-waste  sites,  that  can  be  potentially  harmful  to  humans. 

Present  day  industrial  robots  are  typically  slow  and  massive.  In  general,  they  are  con¬ 
trolled  at  a  joint  level,  and  infer  end-point  position  from  joint-angle  measurements.  This 
control  approach,  which  involves  sensing  and  actuating  at  the  same  physical  location,  is 
known  as  collocated  control.  Obviously  if  any  link  flexibility  exists,  the  end-point  posi¬ 
tion  will  not  correspond  to  the  joint-angle  measurements,  and  performance  will  deteriorate. 
Consequently,  to  avoid  errors  due  to  link  flexibility,  industrial  robots  have  been  designed  to 
be  very  massive  and  rigid. 
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1.1.2  Flexible-Link  Manipulators 

Unfortunately,  because  of  the  cost  of  boosting  mass  into  orbit,  very  massive  and  rigid  space- 
based  manipulators  are  uneconomical  and  therefore  impractical.  In  addition,  space-based 
manipulators  are  designed  to  have  long  links  in  order  to  cover  a  large  workspace.  This 
combination  of  link  characteristics  -  long  and  lightweight  -  results  in  manipulators  that 
exhibit  significant  link  flexibility.  Thus,  when  manipulating  large  payloads  or  operating  at 
high  speeds,  the  links  will  undergo  large  deflections  that  will  reduce  performance  unless 
their  end  point  is  controlled  actively  and  directly. 

The  Shuttle  Remote  Manipulator  System  (SRMS)  shown  in  Figure  1.1  is  an  example 
of  a  flexible-link  space-based  manipulator  currently  in  operation.  Allowing  astronauts 


Figure  1.1:  Shuttle  Remote  Manipulator  System 

The  SRMS  is  a  space-based  manipulator  system  that  is  currently  in  operation. 

Future  applications  of  this  system  will  include  refueling,  repair,  and  inspection  of 
spacecraft.  The  speed  and  accuracy  of  the  SRMS  is  limited  in  part  by  the  ability 
of  its  control  system  to  control  actively  inherent  link  Bexibility. 

to  remain  within  the  safe  and  comfortable  confines  of  the  shuttle,  the  SRMS  has  been 
instrumental  in  performing  numerous  tasks  within  the  shuttle  bay.  Figure  1.1  shows  the 
SRMS  having  just  deployed  the  Hubble  Space  Telescope. 
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The  SRMS  extends  15  meters,  is  made  out  of  light-weight  carbon-composite  material, 
and  has  a  mass  of  450  kg.  It  contains  significant  link  flexibility.  Its  lowest  natural  frequency, 
with  locked  joints,  ranges  from  .04  to  .4  Hz  depending  on  the  payload  ([1]  and  [2]), 

1.1.3  Control  of  Flexible-Link  Manipulators 

Currently,  flexible-link  manipulators  like  the  SRMS  are  controlled  at  a  joint  level  using 
a  collocated  control  approach  similar  to  most  industrial  robots.  Because  this  joint-based 
approach  is  very  limited  in  its  ability  to  control  the  end  point  via  the  link  flexibility,  the 
SRMS  must  be  moved  very  slowly.  Typically,  end-point  speeds  are  kept  below  0.06  m/sec 
for  the  fully  loaded  configuration  and  below  0.6  m/sec  for  the  unloaded  configuration  [3]. 

Previous  research  has  demonstrated  that  the  performance  of  flexible  manipulators  like 
the  SRMS  can  be  increased  through  a  control  system  that  is  improved  in  a  very  fundamental 
way:  by  measuring  the  end-point  position  directly,  flexibility-induced  errors  can  be  reduced, 
and  speed  and  precision  of  performance  can  be  increased  significantly  ([4]  and  [5]). 

Although  end-point  feedback  does  dramatically  improve  performance,  there  are  still 
limitations,  in  terms  of  end-point  control  bandwidth  and  accuracy,  associated  with  actuators 
separated  from  the  manipulator  end  point  by  link  flexibility.  These  limitations  can  be 
attributed  to  two  factors. 

•  First,  the  accuracy  with  which  the  end  point  of  the  manipulator  can  be  positioned  is 
limited  by  the  accuracy  with  which  the  joint  actuators  can  be  positioned.  Fine  end¬ 
point  motion  requires  even  more  precise  joint  motion,  and  no  change  in  link  shape. 

•  Second,  because  the  actuators  are  separated  from  the  end  point  by  a  flexible  link, 
there  is  a  fundamental  time  delay  associated  with  the  actuation  effort  propagating 
down  the  flexible  link.  This  delay  invokes  specific  limits  upon  the  end-point  control 
bandwidth. 

1.1.4  Mini-Manipulators 

An  intuitive  way  of  finessing  the  accuracy  and  bandwidth  limitations  associated  with 
flexible-link  manipulators  -  and  thereby  increasing  achievable  performance  -  is  to  incor¬ 
porate  into  the  hardware  design  a  mini-manipulator,  or  high-bandwidth,  small,  rigid  robot, 
at  the  tip  of  the  manipulator.  The  high-bandwidth  mini-manipulator  can  be  used  for  rapid, 
precise  control  within  a  small  workspace,  while  the  main  manipulator  is  responsible  for 
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transporting  the  mini- manipulator  from  workspace  to  workspace.  The  combination  of  a 
mini-manipulator  and  direct  end-point  sensing  can  therefore  enable  high-performance  con¬ 
trol  at  the  tip  of  a  multi-link  manipulator  despite  significant  link  flexibility. 

The  concept  of  using  a  mini-manipulator  and  end-point  sensing  to  increase  achievable 
performance  motivated  this  dissertation.  Moreover,  this  research  serves  as  a  specific  example 
of  a  more  generic  concept:  the  use  of  local  actuation  and  sensing  to  achieve  high-performance 
control  at  a  specific  location.  This  research  also  focused  on  system-level  optimization  to 
exploit  the  attributes  of  each  subsystem  -  the  long,  limber  main  manipulator  and  the  quick, 
precise  mini-manipulator. 

The  ideas  that  prompted  this  dissertation  are  also  the  motivation  behind  designs  en¬ 
visioned  for  future  space  missions.  For  example.  Figure  1.2  is  an  artist’s  rendition  of  the 


Main  Arm 


Dextrous  Manipulator 

Figure  F2:  Artist’s  Rendition  of  the  Space  Station  Manipulator  Servicer 

This  servicer  might  be  responsible  for  tasks  such  as  maintenance,  assembly,  and 
transportation  about  the  space  station.  To  increase  the  speed  and  precision  with 
which  these  tasks  can  be  performed,  a  mini-manipulator  has  been  incorporated  into 
the  hardware  design  of  this  system. 


Space  Station  Manipulator  Servicer  (SSMS).  This  system  contains  a  flexible  main  arm  that 
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is  similar  to  the  SRMS.  In  addition,  the  SSMS  includes  the  Special  Purpose  Dextrous  Ma- 
nipulator  (SPDM),  which  is  a  small,  rigid  robot  that  is  mounted  at  the  tip  of  the  main 
arm.  The  motivation  behind  the  SPDM  is  to  provide  capability  for  local  manipulation  to 
increase  achievable  performance  at  the  tip  of  the  flexible  main  arm. 

1.1.5  Control  of  a  Flexible  Manipulator  with  a  Mini-Manipulator 

The  overall  system  performance  of  a  multi-link  flexible  manipulator  with  a  mini-manipulator 
will  be  limited,  in  part,  by  the  ability  of  the  flexible  main  arm  to  position  quickly  the  mini¬ 
manipulator  within  the  desired  workspace.  This  is  mitigated,  however,  by  the  fact  that  even 
for  very  high-performance  control,  the  flexible  main  arm  need  be  controlled  only  to  an  area, 
rather  than  to  a  point;  an  area  within  which  the  speed  and  precision  of  the  mini-manipulator 
can  then  be  utilized  to  perform  the  task  at  hand. 

High-performance  control,  then,  calls  for  an  advanced  control  system  that  accounts  for 
the  inherently  flexible  links  of  the  main  arm,  exploits  the  quick,  precise,  local-manipulation 
capabilities  of  the  mini-manipulator,  and  exploits  explicitly  the  inherently  greater  speed 
with  which  the  end  of  the  large  flexible  main  arm  can  be  positioned  to  within  an  area  rather 
than  it  can  be  positioned  to  a  point  This  is  a  generic  control-system  concept  that  can  enrich 
a  variety  of  future  automatic  control  systems  that  deliberately  incorporate  redundancy. 

In  developing  this  advanced  control  system,  a  number  of  very  complex  issues  must  be 
addressed: 

•  Control  Strategy:  Development  of  a  specific  system-control  strategy  is  required  to 
exploit  the  combination  of  high-bandwidth,  local-manipulation  benefits  of  the  mini¬ 
manipulator  with  the  peculiar  benefits  of  large-slew  control  of  the  flexible  main  arm 
to  a  terminal  area  (rather  than  to  a  point). 

•  Control  Approach:  Formulation  of  the  control  strategy  in  the  form  of  a  specific 
control  approach  is  needed. 

•  Modelling:  Equipping  a  multi-link  flexible  manipulator  with  a  mini-manipulator 
results  in  an  extremely  flexible,  dynamically  coupled,  redundant  system.  Controlling 
this  system  requires  first  developing  an  accurate  mathematical  model  to  support  fully 
control-system  design,  simulation,  and  analysis. 
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®  Sensor  Selection:  Exploiting  the  benefits  of  the  mini-manipulator,  and  achieving 
high-performance  control  of  the  main  arm  in  the  presence  of  link  flexibility,  requires 
end-point  sensing  of  position  and  orientation. 

®  Estimation:  Because  it  is  not  at  all  practical  to  measure  all  states  of  such  a  system, 
state  estimation  of  the  nonlinear  multi-link  main-arm/mini-manipulator  system  is 
required  for  those  states  that  are  not  measured. 

®  Multi-Link  Systems:  Each  of  the  above  issues  becomes  considerably  more  com¬ 
plicated  for  manipulators  with  many  flexible  links.  Tasks  in  which  a  manipulator 
must  grasp  an  object  in  various  configurations  or  tasks  that  require  a  manipulator  to 
maneuver  around  a  space  structure  are  applications  where  a  manipulator  with  many 
links  would  be  required. 

The  goal  of  this  research  is  to  develop  the  basic  theory  and  technology  to  enable  high- 
performance  end-point  control  of  multi-link  flexible  manipulators  with  local  end-point  ma¬ 
nipulation  (systems  which  embody  the  issues  just  described). 

1.2  Background 

This  section  reviews  some  of  the  previous  fundamental  research  in  the  control  of  flexible 
manipulators  and  of  mini-manipulators.  It  is  by  no  means  intended  as  a  complete  liter¬ 
ature  review;  subsequent  chapters  discuss  relevant  research  in  much  greater  detail.  Note 
that  none  of  this  previous  research  has  addressed  how  to  incorporate  the  benefits  of  a 
mini-manipulator  into  a  control  approach  for  multi-link  flexible  manipulators.  Hence,  this 
dissertation  addresses  a  totally  new  research  area. 

1.2.1  Flexible-Link  Manipulators 

Eric  Schmitz  performed  at  the  Stanford  Aerospace  Robotics  Laboratory  (ARL)  the  first 
experimental  research  in  the  end-point  control  of  flexible  manipulators.  Through  experi¬ 
ments  with  a  single-link  flexible  manipulator  ([6]  and  [4]),  he  demonstrated  that  end-point 
position  feedback  enabled  a  factor  of  four  improvement  over  the  performance  achievable 
with  conventional  joint-based  collocated  control. 

Schmitz’s  work  led  to  a  sequence  of  experimental  research  achievements  in  the  con¬ 
trol  of  a  single-link  flexible  manipulator.  Maples  [7]  demonstrated  end-point  force  control. 
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Rovner  [8]  showed  that  end-point  control  relied  on  an  accurate  system  model.  He  developed 
an  adaptive  algorithm  based  on  a  self-tuning  regulator  approach  which  could  successfully 
identify  the  tip  payload  mass  in  real  time.  Alder  [9]  developed  an  adaptive  approach  to 
identify  and  control  dynamic  payloads  which  had,  themselves,  a  single,  lightly-damped  os¬ 
cillatory  mode. 

Oakley  ([10],  [11],  [12],  [13],  [14],  and  [5])  made  a  significant  advancement  at  the  ARL  by 
extending  Schmitz’s  findings  to  a  two-link  flexible  manipulator.  She  demonstrated  experi¬ 
mentally  similar  increases  in  performance  using  end-point  feedback  to  control  an  extremely 
flexible  two-link  manipulator. 

Since  the  work  of  Schmitz  and  Oakley,  researchers  at  numerous  universities  and  research 
centers  have  been  studying  various  aspects  of  flexible-link  manipulators.  This  work  ranges 
from  the  computation  of  control  inputs  to  follow  a  specified  trajectory  (  [15],  [16],  [17], 
and  [18])  to  filtering  command  inputs  to  reduce  residual  vibrations  (  [19],  [20],  [21],  [22], 
and  [23]). 

1.2.2  Mini-Manipulat  or  s 

Other  previous  research  at  the  ARL  has  incorporated  the  use  of  a  mini-manipulator  to 
increase  the  performance  of  a  single-link  flexible  manipulator.  The  research  of  Chaing, 
Tilley,  and  Kraft  ([24],  [25],  and  [26])  demonstrated  an  order-of-magnitude  improvement 
in  position  and  force  control  over  that  achievable  with  the  single-link  flexible  manipulator 
alone.  Furthermore,  Andersen  [27]  utilized  a  mini-manipulator  to  demonstrate  precise  force 
control  at  the  end  of  a  two-link  manipulator  with  flexible  drive  trains. 


1.3  Research  Goals 

There  were  two  primary  objectives  of  the  research  presented  in  this  dissertation: 

1.  To  develop  a  control  approach  that  exploits  in  a  system-optimal  way  the  benefits  of 
a  mini-manipulator  to  achieve  high-performance  end-point  control  of  multi-link,  very- 
flexible  manipulators. 

2.  To  verify  experimentally  this  approach  on  the  two-link  flexible  manipulator  with  a 
mini-manipulator  mounted  at  its  tip  as  shown  in  Figure  1.3. 
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Figure  1.3:  Experimental  Apparatus 

The  experimental  apparatus  used  in  this  research  consisted  of  an  extremely  Sexible 
two-link  manipulator  with  a  mini-manipulator,  or  small  rigid  robots  mounted  at 
its  tip.  This  experimental  system  was  used  to  verify  and  refine  the  theoretical 
developments  presented  in  this  dissertation. 


L4.  GENERAL  APPROACH 


9 


The  specific  problem  addressed  in  this  research  was  to  achieve  high-performance  point- 
to-point  end-point  control  of  the  two- link  main- ar m/mini-manipulator  system.  A  schematic 
of  this  task  is  shown  in  Figure  1.4.  The  point-to-point  repositioning  was  chosen  as  an 


Point-To-Point  Repositioning 


Initial  Configuration 

(t  =  0) 


Final  Configuration 


Figure  1.4:  Experimental  Demonstration 

A  point-to-point  repositioning  was  chosen  as  an  experimental  demonstration  of  high- 
performance  end-point  control  of  the  Qexible  main-arm/mini-manipulator  system. 
With  the  system  in  an  initial  conSguration,  a  desired  target  is  specified  along  with 
a  final  time  in  which  the  target  is  to  be  acquired.  Meeting  successfully  the  point-to- 
point  positioning  requirements  involves  positioning  the  tip  of  the  system  over  the 
target  within  the  specified  time,  and  then  maintaining  this  tip  position  precisely, 
despite  disturbances,  until  a  new  target  is  specihed. 


experimental  task  for  two  reasons.  First,  it  is  a  generic  task  in  that  it  is  representative 
of  many  other  tasks:  pick-and-place  maneuvers,  repositioning  of  flexible  space  structures, 
transportation  of  payloads,  and  precision  pointing  of  flexible  spacecraft.  Second,  successful 
point-to-point  repositioning  requires  both  high-performance  large-motion  slews  of  the  entire 
system  as  well  as  high-performance  end-point  control  within  a  localized  workspace. 


1.4  General  Approach 

To  exploit  both  the  high-bandwidth  local-manipulation  capability  and  the  redundancy  in¬ 
troduced  by  the  mini-manipulator,  and  the  fact  that  the  flexible  main  arm  can  be  positioned 
much  more  quickly  to  be  within  an  area  than  it  can  be  positioned  to  be  at  a  point,  a  new 
control  strategy  for  controlling  multi-link  flexible  manipulators  has  been  developed.  Because 
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the  mini-manipulator  provides  quick,  precise  control  only  within  a  localized  workspace,  sys¬ 
tem  performance  for  large-motion  slews  is  limited  by  the  speed  at  which  the  flexible  main 
arm  can  transport  the  mini-manipulator  to  the  area  where  it  is  to  work.  As  a  result,  meeting 
the  goals  of  this  research  resulted  in  a  two-part  control  strategy: 

®  To  control  the  main  arm  quickly  to  a  target  area^  rather  than  to  a  precise  location. 
This  new  concept  of  controlling  to  a  target  area  is  pursued  in  this  research  through  a 
new  technique  known  as  soft  terminal  control 

»  To  control  the  mini-manipulator  tip  precisely  to  the  desired  target  position  (a  point 
in  space). 


Soft  terminal  control  of  multi-link  flexible  manipulators  is  made  possible  by  the  speed  and 
redundancy  introduced  by  the  mini-manipulator.  Because  of  this  redundancy,  the  target 
can  be  acquired  with  the  system  in  more  than  one  configuration.  Figure  1.5  illustrates  this 
concept.  Rather  than  controlling  the  flexible  main  arm  to  a  specific  point,  successfully 


Target  Area 


Figure  1.5:  New  Performance  Objective 

Because  of  the  redundancy  and  higEbandwidth-manipulation  capability  introduced 
by  the  mini-manipulatorj  meeting  successfully  the  requirements  of  a  point-to-point 
repositioning  depends  first  upon  controlling  quickly  the  main  arm  to  a  target  area. 
The  target  area  is  defined  as  the  region  from  which  the  mini-manipulator  can  acquire 
the  desired  target.  The  control  philosophy  used  for  controlling  to  a  target  area, 
rather  than  to  a  specific  point,  is  a  fundamentally  new  approach  to  controlling 
multi-link  flexible  manipulators.  It  is  known  as  soft  terminal  control. 
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meeting  the  repositioning  requirements  necessitates  controlling  the  main  arm  near  the  tar¬ 
get,  to  a  target  area.  Then,  once  near  the  target,  by  taking  advantage  of  the  speed  and 
redundancy  introduced  by  the  mini-manipulator,  the  mini-manipulator  can  acquire  and 
maintain  the  desired  tip  position  very  quickly  and  precisely. 

To  position  quickly  the  flexible  main  arm  to  the  required  area  near  the  desired  target, 
a  new  soft-terminal-control  approach  for  multi-link  flexible  manipulators  was  developed.  A 
block  diagram  of  the  solution  framework  in  which  the  approach  was  implemented  is  shown 
in  Figure  1.6.  This  framework  merges  a  new  trajectory-generation  approach  and  a  novel 
feedback-control  approach  to  achieve  high-performance  control.  Although  the  structure  of 
this  framework  is  not  new,  the  novelty  stems  from  the  feedforward- trajectory  generation 
and  the  feedback-control  approaches  developed  in  this  research. 

This  control-system  framework  is  based  upon  two  components:  the  feedforward-tra¬ 
jectory  generator  and  the  feedback  controller.  The  trajectory  generator  takes  as  input 
the  reference  commands,  and  produces  the  desired  plant-state  history  and  the  necessary 
feedforward-control  input  to  the  actuators  to  effect  this  desired  motion.  The  feedback 
controller  incorporates  measurements  of  plant  outputs  to  produce  a  feedback-control  input 
to  the  actuators.  This  feedback  control  combines  with  the  feedforward  control  to  regulate 
the  plant  to  the  desired  plant  state  in  the  presence  of  modelling  errors  and  disturbances. 

In  addition  to  the  trajectory-generation  and  feedback-control  approaches,  a  new  mod¬ 
ular  modelling  technique  has  been  developed  in  conjunction  with  Steve  Ims  and  Larry 
Alder  [28].  This  technique  has  enabled  a  modular  approach  to  modelling  multi- link  flexible 
manipulators.  The  primary  advantage  of  this  approach  is  that  it  enables  the  modelling  of 
multi-link  flexible  manipulators  to  be  partitioned  into  modelling  smaller  subsystems,  and 
then  integrating  the  subsystem  models  to  obtain  a  total-system  model. 

The  next  subsections  give  brief  descriptions  of  new  approaches  developed  in  this  disser¬ 
tation  to  each  of  the  three  steps  in  the  design  process:  feedforward- trajectory  generation, 
feedback-control  design,  and  modelling.  Chapters  4,  5,  and  3  each  present  a  detailed  de¬ 
scription  of  the  new  approaches  to  one  of  these  steps. 

1.4.1  Feedforward  Trajectory  Generation 

Trajectory  generation  is  paramount  to  achieving  high-performance  point-to-point  reposi¬ 
tioning.  In  generating  and  effecting  a  complete  trajectory  for  a  point-to-point  repositioning, 
there  are  two  components  that  need  to  be  computed.  The  first  is  the  desired  state  history,  or 
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Feedforward 

Control 


Figure  1.6:  Control  Framework 

The  control  framework  developed  in  this  dissertation  merges  a  new  feedforward 
trajectory-generation  scheme  with  feedback  control  to  achieve  high-performance 
soft-terminal-control  of  multi-link  flexible  manipulators.  The  trajectory  generator 
computes  the  necessary  feedforward-control  input  so  that  the  plant  follows  the  ref¬ 
erence  commands  with  the  corresponding  desired  plant-state  history.  The  feedback 
controller  uses  measurements  from  the  plant  to  produce  a  feedback-control  com¬ 
ponent.  This  feedback  control  augments  the  feedforward  control  to  assure  that 
the  plant  follows  the  desired  plant  state  in  the  presence  of  modelling  errors  and 
disturbances. 


the  desired  plant  motion  for  the  repositioning.  The  second  component  is  the  corresponding 
feedforward-control  input  to  the  actuators  that,  in  the  presence  of  an  exact  model,  causes 
the  desired  motion. 

There  are  three  issues  that  make  trajectory  generation  for  multi-link  flexible  manipula¬ 
tors  a  challenging  task.  First,  for  real-time  implementation,  the  trajectory  generation  must 
be  numerically  efficient  to  mitigate  the  computation-invoked  lag  time  at  the  beginning  of 
the  trajectory.  Second,  finding  the  actuator-command  history  that  corresponds  to  a  desired 
trajectory  requires,  in  general,  inverting  a  high-order  plant  ^  for  every  trajectory  to  be  fol¬ 
lowed.  Because  flexible  manipulators  are  non-minimum-phase  systems,  plant  inversion  is  a 
computationally  intensive  task.  Finally,  because  high-frequency  inputs  can  excite  lightly- 
damped  or  unmodelled  flexible  modes,  the  frequency  content  of  the  prescribed  trajectory 
should  be  shaped  carefully. 

The  trajectory-generation  approach  developed  in  this  dissertation  is  based  on  a  model¬ 
following  control  framework  in  which  a  plant  model  is  driven  by  the  time-varying  gains  of  a 
terminal  controller.  The  trajectory  generator  produces,  in  real  time,  the  optimal  trajectory 


^The  plant’s  flexible-beam  components  result  in  an  infinite  number  of  natural  modes  of  vibration. 
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for  a  finite-time  point-to-point  repositioning.  Included  in  the  computed  trajectory  are  both 
the  optimal  state  history  and  the  corresponding  feedforward-control  inputs  to  the  actuators. 

Benefits 

The  primary  benefit  of  this  approach  is  that  it  provides,  in  real  time,  an  optimal  trajec¬ 
tory  for  the  flexible  main  arm  to  follow  during  a  point-to-point  repositioning  of  the  main- 
arm/mini-manipulator  system.  In  addition  to  performance  benefits,  there  are  several  other 
characteristics  of  this  trajectory-generation  approach  that  make  it  valuable  in  controlling 
multi-link  flexible  manipulators  with  local-manipulation  capability. 

•  First,  this  approach  exploits  the  speed  and  redundancy  introduced  by  the  mini¬ 
manipulator  which  makes  it  necessary  only  to  quickly  bring  the  main  arm  near  the 
desired  target.  To  do  this,  the  strategy  of  soft  terminal  control  is  incorporated  into  the 
trajectory  generation  in  the  form  of  soft  terminal  constraints  in  the  terminal-controller 
design. 

•  Second,  the  generated  trajectory  consists  of  both  a  desired  state  history  and  the 
corresponding  feedforward  control  to  the  actuators,  both  of  which  are  computed  in 
real  time. 

•  Third,  the  only  slew-dependent  precomputations  of  this  approach  are  based  on  the 
specified  slew  time.  As  a  result,  for  repositionings  of  the  same  slew  time,  no  additional 
precomputations  are  required. 

•  Fourth,  included  in  this  approach  is  a  method  for  shaping  the  frequency  content  of 
the  trajectory  not  to  excite  lightly-damped  or  unmodelled  flexible  modes. 

This  optimal- trajectory-generation  approach  is  discussed  in  detail  in  Chapter  4. 

1A.2  Feedback  Control 

In  any  real  system,  feedback  control  is,  of  course,  always  essential  to  ensure  that  the  system 
-  in  this  case  a  multi-link  flexible  manipulator  -  follows  the  commanded  state  history. 
The  strength  of  the  feedback  needed  will  be  affected  by  the  magnitude  of  the  process 
uncertainties  and  modelling  errors  present,  among  other  things.  Although  methods  exist 
for  designing  high-performance  controllers,  the  goal  of  the  approach  used  here  is  to  enable 
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a  partitioned  design  process,  while  at  the  same  time  retaining  as  much  performance  as 
possible. 

The  control  approach  developed  in  this  dissertation  enables  the  development  of  high- 
performance  controllers  for  predominantly  one-way  coupled  systems  to  be  partitioned  into 
sequential  subsystem  designs.  This  sequential  process  exploits  the  one-way  coupling  that 
characterizes  the  system  within  the  frequency  range  of  interest  for  control.  Because  the 
one-way  coupling  assumption  is  inaccurate  outside  of  this  frequency  range,  the  subsystem 
controllers  are  designed  to  be  robust  to  this  model  uncertainty.  As  a  result,  this  approach 
enables  the  high-performance- control  design  for  complex  systems  to  be  partitioned  into 
smaller,  more  tractable  subsets. 

Benefits 

There  are  two  advantages  to  this  sequential  design  approach. 

•  First,  because  of  its  sequential  nature,  this  approach  enables  the  development  of  a 
total-system  controller  to  be  separated  into  smaller  and  more  tractable  control-design 
problems.  This  is  particularly  advantageous  in  control  design  for  flexible  manipulators 
with  many  links. 

•  Second,  for  predominantly  one- way-coupled  systems,  the  robust  subsystem  controllers 
enable  high-performance  in  the  presence  of  modelling  uncertainty  outside  the  fre¬ 
quency  range  of  interest  for  control. 

This  feedback-control  approach  is  discussed  in  detail  in  Chapter  5. 

1.4.3  Modelling 

The  trajectory-generation  and  feedback-control  approaches  developed  in  this  dissertation 
require  an  accurate  system  model  for  design,  simulation,  and  analysis.  The  modular  mod¬ 
elling  approach  developed  in  this  research  enables  the  development  of  a  total-system  model 
to  be  partitioned  into  the  modelling  of  smaller,  less  complex  subsystems.  After  accurate 
modelling  of  each  subsystem,  a  total-system  model  is  obtained  through  a  concatenation, 
or  integration,  of  subsystem  state-space  models  by  enforcing  the  physical  constraints  that 
couple  the  subsystems.  ^  This  integration  of  accurate  subsystem  state-space  models  yields 

^This  approach  enables  the  concept  of  model  concatenation,  also  utilized  in  the  research  of  Koning- 
stein  [29],  Chen  [30],  and  Alder  [9],  to  be  applied  to  state-space  subsystem  models.  Consequently,  this 
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an  accurate  total-system  state-space  model  from  which  high-performance  controllers  can  be 
designed. 

Benefits 

This  modular  modelling  approach  has  four  characteristics  that  make  it  particularly  well 
suited  to  modelling  multi-link  flexible  manipulators. 

•  First,  because  of  the  modularity  of  this  approach,  it  is  amenable  to  modelling  com¬ 
plex  systems  in  which  the  subsystems  can  be  constrained  physically  in  a  variety  of 
conflgurations. 

•  Second,  it  allows  for  fast  rederivation  of  total-system  models  in  the  presence  of  sub¬ 
system  modiflcations,  additions,  and  substitutions, 

•  Third,  the  method  allows  the  subsystem  models  to  be  in  state-space  form  without 
restrictions  on  the  elements  of  the  state  vector.  Thus,  the  most  convenient  set  of 
coordinates  can  be  used  to  model  the  subsystems. 

•  Fourth,  this  approach  yields  a  linear-time-invariant  (LTI)  system  model  of  the  form 

x(t)  ==  Ax(t)  -h  Bu(t) 
y{t)  =  Cx{t) 

where  x(t)  is  a  vector  of  state  elements,  n{t)  is  a  vector  of  control  inputs,  y{t)  is  a 
vector  of  outputs,  A  and  B  are  the  state  transition  and  input  matrices,  and  C  is 
the  output  matrix.  This  is  the  model  form  required  by  much  of  the  existing  optimal- 
control  theory  for  LTI  systems. 

1.5  Contributions 

In  meeting  the  goals  of  this  dissertation,  the  research  reported  in  this  thesis  makes  the 
following  contributions  to  the  fields  of  automatic  control  and  robotics: 

approach  enables  Oakley’s  novel  System  Modes  Representation  modelling  approach  [5],  from  which  accurate 
state-space  models  of  flexible  systems  can  be  derived,  to  be  utilized  in  the  subsystem  modelling  process  of 
a  concatenation-based  modelling  approach. 
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A  generic  new  approach  to  achieving  high-performance  control  of  multi-link  very- 
flexible  manipulators  has  been  developed.  This  approach  incorporates  local-manipu¬ 
lation  capability,  in  the  form  of  a  mini-manipulator,  and  direct  end-point  sensing  to 
remove  totally  the  severe  performance  limitations  imposed  by  the  combination  of  link 
flexibility  and  actuation  at  a  distance  from  the  point  to  be  controlled. 

A  new  control  strategy,  soft  terminal  control,  has  been  developed  to  exploit  the  speed 
and  redundancy  of  the  mini-manipulator.  The  specific  objective  of  this  strategy  is  to 
control  quickly  the  flexible  main  arm  to  a  target  area,  rather  than  to  a  specific  point, 
from  which  the  mini-manipulator  can  acquire  the  desired  target.  The  performance 
benefits  of  this  strategy  stem  from  the  dramatic  decrease  in  time  required  to  control 
a  multi-link  flexible  manipulator  to  an  area  rather  than  to  a  specific  point. 

A  fundamentally  new  optimal-trajectory-generation  approach  for  multi-link  flexible 
manipulators  has  been  developed.  This  approach,  based  on  a  model-following  imple¬ 
mentation  of  a  terminal  controller,  incorporates  the  novel  soft-terminal-control  con¬ 
cept  of  controlling  the  end-point  of  the  main  arm  to  a  target  area  by  establishing  soft 
terminal  constraints  for  the  main  manipulator  in  the  terminal-controller  design. 

The  optimal-trajectory  generation  requires  minimizing  a  finite-time,  linear-quadratic 
performance  index  with  soft  terminal  constraints,  resulting  in  time-varying  gains.  The 
generated  trajectory  consists  of  the  optimal  state  history  as  well  as  the  corresponding 
optimal  control-command  history.  This  new  approach  generates  trajectories  that  cor¬ 
respond  to  “near-minimum-time”  repositionings.  Furthermore,  all  of  the  significant 
non-slew-specific  computations  can  be  performed  ahead  of  time,  since  these  compu¬ 
tations  are  independent  of  initial  and  final  configurations.  As  a  result,  this  approach 
offers  benefits  in  terms  of  implementation  and  design. 

A  method  has  been  developed  to  shape  the  frequency  content  of  the  generated  trajec¬ 
tories  not  to  excite  lightly-damped  unmodelled  flexible  modes.  This  method  involves 
frequency  weighting  the  finite-time,  linear-quadratic  cost  function.  As  a  result,  this 
approach  is  directly  applicable  to  manipulators  exhibiting  link  and/or  drive-train  flex¬ 
ibility. 

A  method  for  achieving  an  essentially  bumpless  control-mode  transfer  at  the  end  of 
large-motion  slews  has  been  developed.  This  method  involves  time  weighting  the  cost 
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function  in  conjunction  with  implementing  the  terminal  controller  in  a  model-following 
control  framework. 

•  This  optimal-trajectory-generation  approach  was  tested  successfully  and  verified  on  a 
physical  two-link,  extremely  flexible  manipulator.  These  experiments  demonstrated 
for  the  first  time  the  use  of  a  terminal-controller-based  approach  to  reposition  rapidly 
a  lightly-damped  multi-input-multi-output  flexible  manipulator. 

•  A  feedback-control  approach  has  been  developed  for  a  class  of  multi-link  flexible  ma¬ 
nipulators  containing  subsystems  that  are  predominantly  one-way  coupled.  By  ex¬ 
ploiting  this  limited-coupling  characteristic,  this  control  approach  enables  a  sequen¬ 
tial  design  process.  The  subsystem  controllers  are  made  robust  to  model  uncertainty 
outside  the  frequency  range  of  interest  through  a  frequency-shaped  control  design. 
Because  system-level  coupling  information  is  fully  utilized  in  this  process,  and  not 
overlooked,  high  performance  is  achieved;  yet,  through  a  sequential  control- design 
process. 

•  A  generic  modelling  approach  has  been  developed,  based  on  the  model  concatenation 
technique  presented  in  [28],  that  facilitates  the  modelling  of  a  wide  class  of  multi-link 
flexible  structures.  This  method  enables  the  development  of  a  total-system  model 
through  a  concatenation  of  subsystem  models.  This  approach  is  inherently  modular, 
and  is  extensible  to  any  number  of  links  and  degrees  of  freedom.  This  method  was  used 
to  develop  an  accurate  total-system  model  of  the  two-link-flexible-manipulator/mini- 
manipulator  system. 

•  This  research  culminated  with  the  first-ever  demonstration  of  end-point  control  of  a 
mini-manipulator  mounted  at  the  tip  of  an  extremely  flexible  multi-link  manipulator. 
High-performance  end-point  position  control  was  demonstrated  for  large  slews  of  the 
total  system  and  for  disturbance  rejection  at  the  tip  of  the  mini-manipulator/two-link- 
flexible-manipulator  system.  For  the  system  repositionings  presented  in  this  disser¬ 
tation,  the  mini-manipulator  enabled  nearly  a  factor  of  two  decrease  in  repositioning 
times.  Also,  the  mini- manipulator  reduced  tip  errors  due  to  tip  disturbances  by  a 
factor  of  25. 

•  Each  of  the  above  contributions  represents  valuable  progress  toward  the  general  prob¬ 
lem  of  controlling  a  system  made  up  of  many  (n)  very  flexible  substructures  for  which 
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a  distributed  set  of  many  actuators  can  be  used.  For  the  experiments  presented  in 
this  dissertation,  n  =  3,  which  connotes  considerable  generality. 


1.6  Reader’s  Guide 

This  chapter  has  served  as  an  introduction  to  the  research  that  is  presented  in  this  disser¬ 
tation.  The  remainder  of  this  thesis  is  organized  as  follows: 

Chapter  2  gives  a  description  of  the  experimental  apparatus.  Included  in  this  chapter 
is  a  detailed  description  of  the  two-link  flexible  manipulator  and  of  the  mini-manipulator 
mounted  at  its  tip,  as  well  as  a  description  of  the  real-time  computer  hardware  and  software 
environment. 

In  Chapter  3,  the  generic  modular  modelling  approach  is  described.  This  chapter  also 
contains  a  specific  application  of  this  approach  to  modelling  the  two-link  flexible  manipu¬ 
lator  with  the  mini-manipulator  mounted  at  its  tip. 

Chapter  4  describes  the  new  optimal- trajectory-generation  approach.  To  illustrate  the 
trajectory-generation-design  process,  optimal  trajectories  are  generated  and  presented  for 
the  flexible  main  arm. 

Chapter  5  contains  a  description  of  the  sequential  control  approach  for  predominantly 
one-way-coupled  systems.  Also  included  in  this  chapter  is  an  application  of  this  control 
approach  to  the  experimental  apparatus. 

Chapter  6  integrates  the  optimal-trajectory  generator  of  Chapter  4  and  the  feedback 
controller  of  Chapter  5  to  demonstrate  high-performance  point-to-point  repositionings  of  the 
experimental  mini-manipulator/main-arm  system.  The  necessary  contributions  of  feedback 
and  feedforward  control  in  meeting  successfully  the  system  repositioning  requirements  are 
illustrated.  Also  presented  in  this  chapter  is  a  description  of  the  graphical  user  interface  to 
the  experimental  apparatus. 

Chapter  7  concludes  this  dissertation  with  a  summary  of  results  and  recommendations 
for  future  research. 

Appendix  A  presents  the  State-Space  Model  Concatenation  Method  upon  which  the 
modular  modelling  approach  is  based. 

Appendix  B  provides  important  kinematic  relations  for  the  mini-manipulator. 

Appendix  C  details  the  process  of  modelling  the  flexible  links  of  the  main  arm. 
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Appendix  D  gives  details  of  the  terminal-controller  solution,  an  example  illustrating 
the  performance  advantages  of  time-varying  gains,  and  the  software  used  to  generate  the 
optimal  trajectories. 

Appendix  E  presents  the  estimator  design  for  the  flexible  main  arm. 

Appendix  F  provides  the  specific  model  and  controller  matrices  that  were  used  in  this 
research. 
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This  chapter  gives  a  description  of  the  experimental  apparatus  used  to  verify  and  refine  the 
theoretical  developments  of  this  dissertation.  Included  in  this  chapter  is  a  description  of 
the  hardware,  the  real-time-computer  hardware  and  software  environment,  and  the  analysis 
software  tools  that  were  used  in  this  research. 

2.1  Design  Philosophy 

This  section  gives  a  brief  description  of  the  philosophy  that  governed  the  system  design  of 
the  two-link  flexible  manipulator  with  the  mini-manipulator  shown  in  Figure  2.1. 

2.1.1  Flexible  Manipulator 

The  experimental  manipulator  was  constructed  to  serve  as  a  test-bed  for  evaluating  and 
refining  new  theoretical  developments  for  the  control  of  flexible  manipulators.  The  overall 
design  strategy  was  to  keep  the  design  simple,  yet  to  exaggerate  link  flexibility  for  research 
purposes. 

The  flexible  main  arm  was  designed  so  that  damping  must  be  provided  through  active 
control.  The  dimensions  of  each  of  the  elastic  links  were  chosen  specifically  to  exaggerate 
their  structural  flexibility  so  that  the  flexibility  could  not  be  ignored.  Along  with  exagger¬ 
ating  the  link  flexibility,  the  mechanical  structure  was  designed  to  be  lightly  damped. 

To  isolate  the  effects  of  bending-induced  flexibility,  significant  structural  bending  is 
limited  to  one  plane.  The  two-link  flexible  manipulator  operates  in  the  horizontal  plane. 
Torsional  stiffness  is  provided  for  both  links  by  the  air-cushion  supports  at  the  manipulator 
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Figure  2.1:  Experimental  Apparatus 

This  is  a  photograph  of  the  experimental  manipulator  system  used  to  evaluate  and 
refine  the  new  theoretical  developments  of  this  research. 
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elbow  and  end  point.  The  link  cross  sections  were  chosen  to  be  relatively  stiff  in  the  vertical 
plane. 

Motors  located  at  the  shoulder  and  elbow  joints  provide  actuation  to  the  manipulator. 
To  keep  the  vibration  frequencies  of  the  second  link  low  without  adding  unnecessary  mass 
to  the  elbow  joint,  the  elbow  motor  is  part  of  the  second  link’s  hub  rather  than  the  first 
link’s  tip.  Currently,  sensors  measure  the  shoulder  and  elbow  joint  angles  and  rates  and  the 
manipulator  end-point  position  and  orientation. 

2.1.2  Mini-Manipulator 

The  design  of  the  mini-manipulator  was  governed  by  the  need  for  fast,  precise  end-point 
manipulation.  The  design,  while  kinematically  and  dynamically  complex,  is  very  elegant 
mechanical^.  Rather  than  having  the  actuators  and  sensors  mounted  on  the  links  where 
they  would  slow  down  the  end-point  response,  they  are  located  at  the  base  of  the  mini¬ 
manipulator.  The  links  can  therefore  be  made  much  lighter  for  quicker  tip  response. 

2.2  Hardware  Description 

This  section  describes  the  two-link  flexible  manipulator  and  the  mini-manipulator.  A 
schematic  of  the  experimental  apparatus  is  shown  in  Figure  2.2. 

2.2.1  Flexible  Manipulator 

The  flexible  main  arm  is  a  two-link  flexible  manipulator  that  operates  on  air  cushions  in 
the  horizontal  plane  of  a  1.2  m  by  2.4  m  granite  table.  Each  of  the  flexible  links  is  0.52 
m  in  length,  and  consists  of  an  aluminum  beam  (cross  section  1  mm  by  38.1  mm)  with 
discrete  masses  evenly  spaced  along  its  length.  The  discrete  masses,  or  mass  intensifiers, 
increase  the  overall  beam  mass  without  changing  its  flexural  rigidity,  and  lower  the  natural 
frequencies  of  vibration. 

The  shoulder  motor,  mounted  on  the  side  of  the  granite  table,  can  provide  a  peak  torque 
of  5.43  N-m.  The  elbow  motor,  mounted  on  the  elbow  air-cushion  pad,  can  provide  a  peak 
torque  of  1.06  N-m.  The  motors  are  direct-drive,  DC  limited-angle  torquers.  Rotary- 
variable-differential-transformers  (RVDT’s)  are  located  at  each  of  the  motor  shafts  and 
provide  joint-angle  measurements.  A  vision  sensor,  fully  described  in  [31],  provides  end¬ 
point  measurements.  It  consists  of  a  CCD  television  camera  that  tracks  a  special  variable 
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Figure  2.2:  Experimental  Hardware  Schematic 

Shown  in  this  figure  is  a  schematic  of  the  two-link  Bexible  manipulator  with  a  mini- 
manipulator  mounted  at  its  tip. 


reflectivity  target  located  at  the  manipulator  end  point.  The  vision  system  has  the  capability 
to  track  multiple  targets  at  a  sample  rate  of  60  Hz  with  a  resolution  of  approximately  1  mm 
over  the  roughly  1.5  m^  workspace  [5].  A  detailed  description  of  the  flexible  manipulator’s 
parameters  is  given  in  Oakley’s  dissertation  [5]. 


2.2.2  Mini-Manipulator 

A  two  degree  of  freedom  mini- manipulator  is  mounted  at  the  tip  of  the  two-link  flexible  main 
arm.  The  mini-manipulator,  shown  in  Figure  2.3,  consists  of  a  five-link,  closed-kinematic 
chain  which  operates  in  the  horizontal  plane.  The  base,  inner,  and  outer  links  of  the  mini¬ 
manipulator  are  approximately  5.1  cm,  7.6  cm,  and  10.2  cm  in  length  respectively.  To  allow 
for  quick,  precise  motion  of  the  tip,  the  four  moving  links  are  made  of  hollow  tubes,  and  are 
actuated  by  two  small  DC  electric  motors  located  at  the  base  of  the  mini-manipulator.  The 
motors  have  a  peak  torque  of  0.35  N-m.  Rotary  encoders  mounted  on  top  of  the  motors 
provide  angular-position  and  velocity  information. 
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Figure  2.3:  Magnified  View  of  the  Mini-Manipulator 
This  figure  illustrates  the  mini-manipulator  and  its  various  components. 


2.3  Computer  System 

The  ARL  has  a  user-friendly,  state-of-the-art,  heterogeneous  computing  environment.  The 
computer  system  consists  of  Sun  engineering  graphics  workstations  interconnected  by  Eth¬ 
ernet,  with  a  central  file  server.  The  real-time  computing  systems  are  also  connected  on 
the  Ethernet.  This  arrangement  has  many  advantages.  For  example,  any  real-time  system 
can  be  accessed  from  any  workstation,  and  the  real-time  systems  can  access  files  on  the  file 
server. 

2.3.1  Real-Time  Software 

The  controllers  discussed  in  this  dissertation  were  implemented  on  a  digital  computer.  The 
real-time  operating  system  was  VxWorks  [32].  In  conjunction  with  VxWorks,  Control- 
Shell  [33]  was  used  for  real-time-software  development.  ControlShell  enables  a  modular 
design  and  implementation  of  real-time  software  which  allows  a  unique  component-based 
approach  to  real-time-software  generation  and  management. 

2.3.2  Analysis  Software 

MATLAB  [34],  a  matrix  manipulation  package,  was  used  for  a  wide  variety  of  analysis 
tasks.  For  example,  it  was  used  to  calculate  and  discretize  system  matrices  of  linearized 
plants,  to  find  eigenvalues  and  eigenvectors,  to  design  controller  gains,  to  check  algorithms, 
to  plot  data,  and  to  perform  simulations.  In  addition,  StethoScope  [35],  was  used  to  collect 
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and  monitor  time  traces  of  variables  on  the  real-time  system.  The  data  collected  using 
StethoScope  was  stored  in  MATLAB  format  for  subsequent  viewing  and  analysis. 

2.3.3  Real-Time  Hardware 

The  real-time  hardware  consisted  of  two  Motorola  MVME-167  real-time  processors,  a  Mo¬ 
torola  MVME-147  real-time  processor,  a  Burr-Brown  MPV954  digital-to-analog  converter 
card,  a  Burr-Brown  MPV950  analog-to-digital  converter  card,  a  DataCube  MaxScan  video 
frame  grabber,  and  a  DataCube  RoiStore  video  frame  memory.  This  real-time  system  hard¬ 
ware  configuration  was  chosen  for  its  easy  expandability  and  for  its  compatibility  with  the 
Sun  processors  and  VME  bus. 


Chapter  3 


Modelling 


This  chapter  presents  a  modular  approach  to  modelling  multi-link  flexible  manipulators. 
This  approach  is  based  on  a  new  model  concatenation  technique,  the  State-Space  Model 
Concatenation  Method,  that  was  developed  in  [28].  Included  in  this  chapter  is  a  specific 
application  of  this  approach  to  modelling  the  experimental  two-link  flexible  manipulator 
with  a  mini-manipulator  at  its  tip. 

Chapters  4  and  5  require  a  system  model  for  trajectory  generation  and  feedback-control 
design.  Specifically,  a  linear-time-invariant  (LTI)  model  is  needed  of  the  form 

x(t)  =  Ax(t)  -h  Bu(t) 
y{t)  =  exit) 

where  x(i)  is  a  vector  of  state  elements,  u(^)  is  a  vector  of  control  inputs,  y{t)  is  a  vector  of 
outputs,  A  and  B  are  the  state  transition  and  input  matrices,  and  C  is  the  output  matrix. 
The  approach  described  in  this  chapter  is  one  possible  technique  that  can  be  used  to  develop 
such  a  model. 

The  modularity  of  the  modelling  approach  presented  here  has  two  distinct  benefits: 

•  First,  modularity  allows  modelling  complex  systems  to  be  partitioned  into  modelling 
smaller  subsystems. 

•  Second,  a  modular  approach  enables  fast  model  rederivation  in  the  presence  of  sub¬ 
system  modifications,  additions,  or  subtractions,  without  rederiving  models  for  the 
unaffected  subsystems. 
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The  modelling  approach  described  in  this  chapter  enables  modelling  of  multi-link  flexible 
manipulators  to  be  partitioned  into  modelling  smaller,  less  complex  subsystems.  After  accu¬ 
rate  modelling  of  each  subsystem,  a  total-system  model  is  obtained  through  a  concatenation, 
or  integration,  of  subsystem  state-space  models  by  enforcing  the  physical  constraints  that 
couple  the  subsystems.  This  integration  of  accurate  subsystem  state-space  models  yields 
an  accurate  total-system  state-space  model  from  which  high-performance  controllers  can  be 
designed. 

Benefits  Because  of  its  modularity,  this  modelling  approach  offers  the  two  benefits  listed 
above.  In  addition,  this  approach  has  two  characteristics  that  make  it  particularly  well 
suited  to  modelling  multi-link  flexible  manipulators  for  the  purposes  of  control  design,  sim¬ 
ulation,  and  analysis. 

•  First,  because  of  its  modularity,  this  approach  is  amenable  to  modelling  complex 
systems  in  which  the  subsystems  can  be  physically  constrained  in  a  variety  of  config¬ 
urations. 

•  Second,  this  approach  allows  the  subsystem  models  to  be  in  state-space  form  where 
the  state  vector  is  arbitrary,  thus  allowing  the  most  convenient  set  of  coordinates  to 
be  used  in  subsystem  modelling. 

3.1  Background 

The  idea  of  model  concatenation  is  common  to  many  well  known  modelling  approaches. 
For  instance,  transmission  line  theory  is  a  modelling  technique  where  subsystem  models,  in 
the  form  of  transfer  function  matrices,  are  concatenated  through  matrix  multiplication  to 
determine  an  input-output  mapping  from  one  point  in  the  system  to  another  [36]. 

In  addition,  the  finite-element  method  is  based  on  model  concatenation.  The  finite- 
element  modelling  technique  treats  a  system  as  an  assemblage  of  elements.  By  imposing 
constraints  that  the  elements  are  represented  by  consistent  generalized  coordinates,  and  that 
the  displacements  be  equal  and  the  forces  in  balance  at  the  element  interfaces,  the  system 
equations  of  motion  can  be  derived  by  concatenating  the  individual  elements’  equations  of 
motion  [37]. 

Other  model-concatenation-based  techniques  have  recently  been  developed.  References 
[29],  [30],  [38],  [39],  and  [40]  give  examples  of  concatenation-based  modelling  techniques. 
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Reference  [9]  presents  a  method  for  concatenating  linear-time-invariant  models  when  the 
subsystem  models  share  generalized  coordinates  at  the  shared  interface.  This  is  an  extension 
of  the  finite  element  philosophy  in  that  only  the  shared  generalized  coordinates  must  be 
common  to  both  subsystem  models.  In  addition,  this  method  differs  from  transmission 
line  theory  in  that  it  allows  for  model  concatenation  of  models  expressed  as  second-order 
differential  equations  rather  than  matrices  of  transfer  functions. 

The  State-Space  Model  Concatenation  Method 

The  modelling  approach  developed  in  this  thesis  is  based  on  the  State-Space  Model  Concate¬ 
nation  Method  (SSMCM)  [28]  that  generalizes  the  technique  described  in  [9]  to  allow  the 
concatenation  of  state-space  subsystem  models  without  placing  restrictions  on  the  elements 
of  the  subsystem  state  vectors.  Furthermore,  working  directly  in  state-space  form,  this 
method  enables  a  specification  of  the  state  vector  of  the  resulting  total-system  model  [28]. 

Problem  Statement  The  state-space  model  concatenation  problem  involves  finding  a 
total-system  model  from  models  of  two  subsystems  that  are  coupled  through  a  subsystem 
interface.  The  coupling  is  characterized  in  terms  of  power  transfer  between  the  subsystems 
through  the  subsystem  interface,  using  the  power-conjugate  signal  pairs  effort  and  flow  [41]. 
For  example,  the  power-conjugate  signal  pairs  in  a  mechanical  system  can  be  force  (effort) 
and  velocity  (flow);  in  an  electrical  system,  voltage  (effort)  and  current  (flow). 

A  complete  model  of  each  subsystem  consists  of  a  state-space  realization  describing 
the  relation  from  the  external  inputs  Ui{t)  and  effort  applied  to  the  subsystem  at  the 
subsystem  interface  e^,  to  the  monitored  outputs  z^,  the  flow  of  the  subsystem  measured  at 
the  subsystem  interface  f^,  and  the  displacement,  or  integral  of  the  flow,  at  the  subsystem 
interface  The  objective  is  to  determine  a  state-space  realization  relating  the  external 
inputs  to  the  monitored  outputs. 

The  general  form  of  the  state-space  realization  of  each  subsystem  is  given  by 

Xj  =  AiX^  +  B^Ui(t)  -f 

Zi  =  Cfxi  (3.1) 

fi  =  Cfxi 

^The  SSMCM  does  not  require  the  displacement  at  the  interface  dt  to  be  included.  However,  given  the 
displacement  as  well  as  the  flow,  the  SSMCM  will  perform  a  structured  model  reduction  to  remove  the 
redundant  coordinates  from  the  total-system  state  vector  [28], 
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di  =  Cfxi 

where  the  dimensions  for  subsystem  i,  i  =  1,2,  are  specified  as  the  state  vector  G 
IR”*b  external  input  vector  u{t)i  €  internal  effort  vector  at  the  interface  €  ]R”®S 

monitored  output  vector  Zi  G  internal  flow  vector  at  the  interface  fj  G  and 

internal  displacement  vector  at  the  interface  dj  G  with  Aj,  B",  Bf,  Cf,  C{,  and  Cf 

having  consistent  dimensions.  These  realization  matrices  may  be  time  dependent.  Note 
that  consistency  of  power  flow  between  the  subsystems  requires  that  the  effort  and  flow 
vectors  all  be  of  the  same  dimension. 

Finally,  it  is  assumed  that  the  effort  at  the  subsystem  interface  satisfies  an  action- 
reaction-type  relation.  For  mechanical  systems,  this  would  correspond  to  a  rigid  connection; 
in  the  electrical  case,  a  short-circuit  connection.  The  corresponding  constraints  are  given 

by 


ei 

=  -62 

(3.2) 

fi 

=  f2 

(3.3) 

di 

=  d2. 

(3.4) 

Solution  Form  The  output  of  the  SSMCM  is  a  coupled  realization  of  the  form 


X  =  Ax  -1-  Bu 
y  =  Cx 


(3.5) 

(3.6) 


where  x  G  jg  total-system  state  vector,  A  and  B  are  the  total-system 

transition  and  input  matrices,  C  is  the  total-system  output  matrix,  and  the  system  inputs 
u  and  outputs  y  are  defined  as 


u  = 


ui 

U2 

Zi 

Z2 


(3.7) 

(3.8) 


A  detailed  description  of  the  solution  process  and  the  resulting  solution  form  is  given  in 
Appendix  A. 
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The  underlying  philosophy  of  this  modelling  approach  is  that  the  development  of  a  total- 
system  model  can  be  simplified  by  first  partitioning  the  complex  system  into  a  number  of 
smaller  subsystems,  and  then  modelling  the  individual  subsystems.  The  subsystem  models 
can  then  be  assembled  to  obtain  a  model  of  the  original  physical  system. 

The  first  step  of  this  approach,  therefore,  involves  partitioning  a  system  into  subsystems 
that  are  coupled  through  constraint  forces  at  the  subsystem  interfaces.  For  example,  for  a 
planar,  three-link  manipulator,  the  partitioning  can  be  performed  as  shown  in  Figure  3.1, 
The  three-link  manipulator  with  torque  inputs  can  be  represented  as  three  subsystems  that 
contain  as  inputs  control  torques  and  the  constraint  forces  that  couple  the  subsystems. 


o 


Figure  3.1: 


System  Partitioning 


A 


This  modular  approach  enables  the  modelling  of  a  multi-link  Sexible  manipulator 
to  be  partitioned  into  modelling  subsystems,  and  then  concatenating  subsystem 
models  to  obtain  a  total-system  model  This  figure  illustrates  how  a  three-link 
manipulator  with  torque  inputs  can  be  represented  as  three  subsystems  that  contain 
as  inputs  control  torques  and  the  constraint  forces  that  couple  the  subsystems.  The 
control  torques  are  represented  by  curved  arrows,  while  the  straight  arrows  represent 
the  constraint  forces. 


The  second  step  of  this  approach  requires  developing  state-space  subsystem  models  of 
the  form 

Xj  =  AjXj  +  Bfui{t)  +  B-ej 

Zj  =  Cfxj  (3.9) 

fj  =  C{  Xj 

di  =  Cf^i. 

Note  that  the  elements  of  Xj  can  be  arbitrary  allowing  the  most  convenient  set  of  coordinates 
to  be  utilized  in  the  subsystem  modelling  process. 
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The  third  and  final  step  of  this  approach  involves  assembling  the  individual  subsystem 
models  of  Equation  (3.9)  to  generate  an  LTI  total-system  state-space  model.  The  State- 
Space  Model  Concatenation  Method  performs  this  assembly.  For  the  system  in  Figure  3.1, 
the  SSMCM  takes  as  input  two  adjacent  subsystem  models  and  outputs  a  coupled  realization 
of  the  two  subsystems.  The  SSMCM  can  be  applied  again  to  concatenate  this  coupled 
realization  with  the  third  subsystem  to  obtain  a  total-system  model.  Section  3.3  gives  an 
example  of  this  approach  applied  to  modelling  the  experimental  apparatus. 

3.3  Example:  Modelling  the  Experimental  Apparatus 

Application  of  the  modular  modelling  approach  to  the  experimental  system  enables  a  total- 
system  model  to  be  developed  by  performing  the  following  steps; 

1.  Partition  the  system  into  subsystems. 

2.  Develop  subsystem  models  of  the  form  of  Equation  (3.9). 

3.  Concatenate  the  subsystem  models  using  the  SSMCM  to  obtain  a  state-space  repre¬ 
sentation  of  the  two-link  flexible  manipulator  with  the  mini-manipulator  at  its  tip. 

This  section  gives  a  detailed  description  of  the  system  partitioning,  the  subsystem  modelling 
processes,  and  the  subsequent  concatenation  of  the  subsystem  models. 

3.3.1  System  Partitioning 

Figure  3.2  illustrates  how  the  experimental  apparatus  can  be  represented  as  three  subsys¬ 
tems  coupled  by  constraint  forces  and  control  inputs.  Specifically,  the  flexible  main  arm 
is  represented  by  a  first  link  that  is  pinned  at  its  base  and  contains  control  inputs  from 
both  the  shoulder  and  elbow  motors.  This  link  is  coupled  through  constraint  forces  at  the 
elbow  joint  to  the  second  link.  The  third  subsystem  consists  of  the  mini-manipulator  and 
its  control  inputs,  and  the  mini-manipulator  base  rigidly  constrained  to  the  second  link. 

3.3.2  Subsystem  Modelling 

With  the  three  subsystems  identified,  the  second  step  of  the  modelling  process  requires  de¬ 
veloping  subsystem  state-space  models  of  the  form  of  Equation  (3.9).  This  section  describes 
the  subsystem  modelling  processes. 


3.3.  EXAMPLE:  MODELLING  THE  EXPERIMENTAL  APPARATUS 
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Figure  3.2:  System  Partitioning  of  the  Experimental  Apparatus 

This  modular  approach  enables  the  modelling  of  the  experimental  apparatus  to  be 
partitioned  into  modelling  subsystems,  and  then  concatenating  subsystem  models  to 
obtain  a  total-system  model  This  figure  illustrates  how  the  experimental  apparatus 
can  be  represented  as  three  subsystems:  two  fiexible  links  and  the  mini-manipulator. 
The  control  torques  are  represented  by  curved  arrows,  while  the  straight  arrows 
represent  the  constraint  forces. 


Modelling  the  Mini-Manipulator 


The  mini-manipulator  subsystem  consists  of  a  base  and  four  dynamic  links.  The  base  is 
coupled  to  the  main  arm  through  constraint  forces,  and  the  links  are  actuated  by  two  control 
inputs.  A  schematic  of  the  mini-manipulator  subsystem  is  shown  in  Figure  3.3. 

As  described  in  Chapter  2,  the  mini-manipulator  was  designed  to  have  very  light  links. 
The  lightweight  nature  of  the  links  can  be  exploited  to  simplify  greatly  the  modelling  and 
control  design  for  the  entire  system.  As  Kraft  demonstrated  [26],  the  lightweight  links 
enable  the  dynamics  of  the  mini-manipulator  to  be  represented  accurately  as  a  dynamic  tip 
mass,  mr,  actuated  by  linear  actuators,  Fx  and  Fy.  The  linear  forces  are  related  to  the 
actual  control  torques  through  the  relation 

T  =  J^F  (3.10)' 


where 


(3.11) 

(3.12) 

(3.13) 


and  J  is  the  mini-manipulator  Jacobian  mapping  the  angular  velocities  at  the  joints,  6,  to 
linear  velocities  at  the  tip  of  the  mini-manipulator,  v,  such  that 
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Figure  3.3:  Schematic  of  Subsystem  3:  The  Mini-Manipulator 

This  figure  is  a  schematic  of  the  mini- manipulator  subsystem.  Shown  in  the  figure 
are  the  constraint  forces  that  couple  this  subsystem  to  the  second  link,  as  well  as 
the  mini-manipulator  control  torques. 


Appendix  B  defines  the  Jacobian  and  other  kinematic  relationships  for  the  mini-manipulator. 

A  schematic  of  the  approximate  mini-manipulator  system  is  shown  in  Figure  3.4.  For 
the  mini-manipulator  utilized  in  this  research,  Andersen  [27]  experimentally  determined  an 
effective  tip  mass  of  mp  =  0.065  kg.  Thus,  the  approximate  linear  equations  of  motion  of 
the  mini-manipulator  dynamics  can  be  written  as 


^niB  ~ 
VruB  ~ 
^rriB  ~ 
^rriT  ~ 
yrriT 


TUB  +  rnp 

1 


rriB  +  '^T 
y  (63^  -  Fy 
1  „ 


rriT 

1 


Tflp 


F^) 

Fy) 


(3.15) 

(3.16) 

(3.17) 

(3.18) 

(3.19) 
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Figure  3.4:  Approximate  Mini-Manipulator  Model 

The  complicated  nonlinear  dynamics  of  the  mini-manipulator  can  be  accurately 
represented  as  a  tip  mass  mr  actuated  by  linear  actuators  Fx  and  Fy,  and  a  base 
mass  and  inertia  ms  and  1. 


where  XmB'f  UmB^  Oms  represent  the  translational  and  rotational  motion  of  the  base 
at  the  point  where  it  is  connected  to  the  second  link,  ms  represents  the  mass  of  the  mini¬ 
manipulator  subsystem  minus  the  identified  tip  mass,  mt^  and  I  represents  the  rotational 
inertia  of  the  mini-manipulator  subsystem  about  the  point  of  connection  with  the  second 
subsystem.  XmT  ymr  represent  the  motion  of  the  mini-manipulator  tip  relative  to  its 
base. 

Since  the  purpose  of  the  mini-manipulator  at  the  end  of  the  flexible  arm  is  to  produce 
much  higher  performance  than  is  possible  with  the  flexible  arm  alone,  it  is  reasonable 
and  very  generic  to  assume  that  the  controllers  for  the  two  systems  will  be  bandwidth 
separated  [26].  This  assumption  enables  Equations  (3.15)  through  (3.19)  to  be  simplified 
further  to 


^TUb  ^ 

1 

(3.20) 

1 

rriB  +  tut 

yniB  ^ 

1 

(3.21) 

^3 

rriB  +  ^ 

^TUb  ^ 

1 

(3.22) 

XrriT  ~ 

—Fx 

rriT 

(3.23) 

VniT  ~ 

—  Fy. 

mp 

(3.24) 
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Observations  Equations  (3.20)  through  (3.24)  can  be  expressed  in  the  form  of  Equa¬ 
tion  (3.9)  and  used  in  the  subsystem  concatenation  process  to  develop  a  total-system  real¬ 
ization  of  the  experimental  apparatus.  However,  the  concatenation  process  can  be  facilitated 
by  identifying  and  exploiting  the  physics  inherent  in  Equations  (3.20)  through  (3.24).  The 
physical  interpretation  of  Equations  (3.20)  through  (3.24)  is  that  the  significant  coupling 
effects  of  the  mini-manipulator  on  the  main  arm  are  due  to  the  mini-manipulator  mass 
and  inertia.  Furthermore,  Equations  (3.23)  and  (3.24)  imply  that  the  mini-manipulator-tip 
dynamics  are  decoupled  from  the  base.  Recognizing  and  exploiting  these  physical  character¬ 
istics  can  reduce  the  subsystem  concatenation  process  from  concatenating  three  subsystems 
to  the  concatenation  of  subsystems  one  and  two,  and  then  appending  the  dynamics  in  Equa¬ 
tions  (3.23)  and  (3.24).  Specifically,  if  the  low  frequency  dynamics  (static  mass  and  inertia) 
of  the  mini-manipulator  is  modelled  as  an  effective  tip  mass  and  inertia  of  the  second  link, 
then  the  mini-manipulator  tip  dynamics  of  Equations  (3.23)  and  Equations  (3.24)  can  be 
separated  from  the  main-arm  dynamics.  This  has  two  important  implications.  First,  it  en¬ 
ables  the  mini-manipulator  dynamic  model  to  be  appended  simply  to  a  model  of  the  main 
arm.  Second,  exploiting  this  physical  decoupling  enables  the  control  design  for  the  main 
arm  and  the  mini-manipulator  to  be  performed  independently. 

Hence,  given  that  the  mini-manipulator  mass,  rriB  +  rriT,  and  inertia,  I,  are  included 
in  the  second  subsystem  model  as  a  tip  mass  and  inertia,  the  mini-manipulator  subsystem 
can  be  represented  by 


Xmx 

=  —Fx 

(3.25) 

ruT 

yiriT 

-  —Fy. 

(3.26) 

rriT 

for  the 

mini-manipulator  in 

the  form  of  Equation  (3.9), 

the  mini-manipulator  subsystem  model  is  given  as 


X3  = 


A3X3  -|-B>3(<) 


3.3.  EXAMPLE:  MODELLING  THE  EXPERIMENTAL  APPARATUS 


37 


where 


X3 


U3 


A3 


B 


U 

3 


VniT 

XrriT 

yrriT 

'  ' 

’0010 

0  0  0  1 

0  0  0  0 

_  0  0  0  0 

■  0  o' 

0  0 

0 

771 7" 

0  -A- 


The  outputs  of  interest  are  the  tip  position  relative  to  the  base,  yielding 


and 


Z3  = 


Xmx 

VniT 


Cf 


1000 

0100 


(3.29) 


(3.30) 


(3.31) 


(3.32) 


(3.33) 


(3.34) 


Because  of  the  approximate  decoupling,  concatenating  this  model  with  the  model  of  the 
main  arm  is  equivalent  to  appending  simply  the  mini-manipulator  model  to  the  main-arm 
model. 


Modelling  the  Second  Link 

Figure  3.5  illustrates  the  second-link  subsystem.  It  consists  of  a  rigid  hub,  a  flexible  link,  and 
a  rigid  tip.  The  inputs  to  this  system  are  a  control  torque  at  the  hub,  and  constraint  forces 
at  the  hub  location  where  the  link  is  pinned  to  the  first  link.  Because  the  rigid  tip  includes 
the  mass  and  inertia  of  the  rigidly  connected  mini-manipulator  base,  there  are  no  constraint 
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Figure  3.5:  Schematic  of  Subsystem  2:  The  Second  Link 

This  Sgure  is  a  schematic  of  subsystem  two.  The  inputs  to  this  system  are  a  control 
torque  U2  at  the  hub^  and  constraint  forces  621  and  C22  at  the  hub  location  where  the 
link  is  pinned  to  the  first  link.  Because  the  rigid  tip  includes  the  mass  and  inertia  of 
the  rigidly  connected  mini-manipulator  base,  there  are  no  constraint  forces  at  the 
tip  of  the  second  link.  9o  is  the  nominal  relative  angle  between  the  first  and  second 
link. 


forces  at  the  tip  of  the  second  link.  Also  included  in  the  figure  is  a  reference  angle,  9o,  which 
is  the  nominal  relative  angle  between  the  first  and  second  link.  Appendix  C  details  the 
modelling  of  the  second  link,  based  on  the  finite-element  method,  and  the  development  of 
the  second-subsystem  model  of  the  form 


X2  =  A2X2  +  B2 U2(t)  -h  6262 
Z2  =  C2X2 

f2  =  Cix2 
d2  =  C^X2 


where  the  constraint-force  input  is  given  by 


62 


621 

622 


(3.35) 


(3.36) 


and  the  velocity  (flow)  and  position  (displacement)  at  the  interface,  or  pinned  connection 
with  the  first  link,  are  given  by 
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d2  = 


^21 

^22 


where  fij  and  dij  are  in  the  direction  of  eij  shown  in  Figure  3.5. 


(3.38) 


Modelling  the  First  Link 

Figure  3.6  gives  a  schematic  of  subsystem  one.  This  subsystem  consists  of  a  pinned  rigid 
hub,  and  a  flexible  link  connected  to  a  rigid  tip.  The  inputs  to  this  subsystem  are  control 
inputs  at  both  the  hub  and  tip,  and  constraint  forces  at  the  tip  caused  by  the  pinned 
connection  to  the  second  link.  Appendix  C  details  the  modelling  of  the  first  link,  in  which 


Uj2 


Figure  3.6:  Schematic  of  Subsystem  1:  The  First  Link 
This  figure  is  a  schematic  of  subsystem  one. 


axial  deflections  are  neglected,  and  the  development  of  the  first  subsystem  model  of  the 
form 

xx  —  AjXi  +  Bx  Ui  (t)  +  B^ei 

zi  -  Cfxx  (3.39) 

fi  =  Cfxi 

di  =  Cfxi 


where 


ex  corresponds  to  the 
subsystem  two  in  the 


ux 


nil 

^12 


(3.40) 


scalar  interface  force  on  subsystem  one  at  the  pinned  connection  to 
direction  shown  in  Figure  3.6,  fi  and  di  are  the  corresponding  flow 
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and  displacement  in  the  same  direction,  B"  is  the  corresponding  input  matrix,  and  C{ 
and  Cf  are  the  corresponding  output  matrices.  Because  of  the  convention  assumed  by  the 
SSMCM,  Equation  3.39  must  be  rewritten  so  that  the  interface  variables  satisfy  the  relation 


ei  =  -62 

fi  =  f2 
di  =  d2. 

For  a  nominal  elbow  angle  Oq,  this  relation  can  be  satisfied  by  defining 

fii  cos{6o)  ~ 

=  li 

/i2  J  [  sin{9o)  _ 

dll  cos{0o) 

=  di 

di2  J  [  sin{Oo) 

r  1 

61  =  cos{0o)  sin{0o) 


(3.41) 

(3.42) 

(3.43) 


(3.44) 


(3.45) 


(3.46) 


where  eu  is  the  constraint  acting  on  subsystem  one  in  the  direction  of  e2i,  and  fu  and  du 
are  the  interface  output  of  subsystem  one  in  the  same  direction.  Then,  defining 

fi  =  (3.47) 


(3.48) 


(3.49) 


—  1^  cos{9o)  sin{0Q) 

cos{0o) 

sin{0o) 


cos  (6*0 ) 
sin{0o) 


(3.50) 


(3.51) 


(3.52) 


Equation  (3.39)  can  be  written  in  the  necessary  form  to  be  concatenated  with  subsystem 


AiXi  +  B]*Ui(t)  +  BjOi 
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zi  =  Cfxi  (3.53) 

fi  =  C{xi 

di  =  Cfxi. 

Equation  (3.53)  is  dependent  on  the  nominal  elbow  angle  9q.  However,  for  a  given  config¬ 

uration,  the  resulting  system  model  can  be  obtained  readily  by  concatenating  the  models 
expressed  in  Equations  (3.35)  and  (3.53),  without  rederiving  Equations  (3.35)  and  (3.39). 


3.3.3  Subsystem  Concatenation 

Section  3.3.2  pointed  out  that  the  subsystem  concatenation  process  can  be  reduced  to 
concatenating  models  for  subsystems  one  and  two  to  develop  a  main-arm  model,  and  then 
appending  the  mini-manipulator  tip  dynamics  to  this  realization.  For  a  given  elbow  angle 
00,  the  system  equations  generated  by  the  SSMCM  are  given  as 


X  -  Ax  +  Bu  (3.54) 

y  =  Cx  (3.55) 


where  x  G  jg  total-system  state  vector,  A  and  B  are  the  total-system 

transition  and  input  matrices,  C  is  the  total-system  output  matrix,  and  the  system  inputs 
u  and  outputs  y  are  defined  as 


u  ~ 


ui 

U2 


(3.56) 


y  = 


Zi 

Z2 


(3.57) 


B  and  u  can  be  simplified  since  one  element  of  ui  is  related  to  W2-  Specifically, 


Ui2  =  -U2 


(3.58) 


at  the  pinned  joint.  Consequently,  the  main-arm  equations  of  motion  can  be  written  as 

X  =  Ax  -f-  Bu 


y 


Cx 


(3.59) 

(3.60) 
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where 

(3.61) 

B(:,3) -B(:,2)  ]  ,  (3.62) 

and  B(:,z)  is  the  column  of  B. 

3.3.4  Comparison  with  Experimental  Data 

Because  the  model  of  the  decoupled  mini-manipulator  dynamic  tip  mass  was  already  veri¬ 
fied  [27],  model  verification  requires  comparing  the  model  of  the  main-arm  with  the  exper¬ 
imental  system.  To  verify  the  accuracy  of  the  model  of  the  flexible  main  arm,  comparisons 
were  made  between  the  model  and  the  experimental  apparatus  for  various  nominal  elbow 
angles.  Experimental  responses  were  obtained  from  sine  sweep  tests  using  a  Solartron 
Model  1254  Frequency  Response  Analyzer.  When  necessary,  very-low-gain  proportional  in¬ 
dependent  joint  controllers  were  implemented  to  keep  the  manipulator  from  drifting  during 
the  experiments.  In  these  comparisons,  the  first  four  flexible  modes  were  included  in  the 
model.  Furthermore,  the  parameters  used  in  these  initial  models  were  the  actual  physical 
parameters  given  in  Appendix  C,  and  were  not  altered  to  obtain  a  “best  fit”. 

Comparisons  of  the  collocated  transfer  functions  for  a  45  degree  elbow  angle  are  given 
in  Figures  3.7  and  3.8.  Figure  3.7,  the  shoulder-joint  frequency  response,  shows  good 
agreement  between  the  model  and  experiment  for  both  magnitude  and  phase.  In  this 
response,  the  second  and  fourth  system  modes  are  dominant,  with  a  very  small  contribution 
from  the  first  and  third  system  modes.  This  implies  that  the  first  and  third  modes  are  barely 
excited  by  the  shoulder  motor  in  a  45  degree  configuration.  The  elbow-joint  frequency 
response  in  Figure  3.8  contains  sets  of  double  peaks  in  the  magnitude  plot,  implying  that 
the  elbow  motor  couples  strongly  into  all  the  system  modes.  This  figure  illustrates  that 
the  model  predicts  accurately  the  low-frequency  cantilevered  zero,  and  the  first,  third,  and 
fourth  system  modes. 

Comparisons  of  the  collocated  transfer  functions  for  a  90  degree  elbow  angle  are  given 
in  Figures  3.9  and  3.10.  Figure  3.9  once  again  demonstrates  excellent  agreement  between 
the  model  and  experiment  for  both  magnitude  and  phase.  In  this  90  degree  configuration, 
only  the  second  and  fourth  system  modes  are  present,  implying  that  the  shoulder  motor 
does  not  couple  into  the  first  and  third  system  modes.  Figure  3.10  illustrates  the  accuracy 


u  = 


B 


Ul 

«2 


[b(:,1) 
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Shoulder 


Figure  3.7:  Shoulder-Joint  Frequency  Response:  45  Degree  Elbow  Angle 

This  figure  shows  the  frequency  response,  both  magnitude  and  phase,  correspond¬ 
ing  to  the  shoulder  motor  for  a  45  degree  nominal  elbow  angle.  This  response  is 
dominated  by  the  second  and  fourth  system  modes.  These  plots  show  excellent 
agreement  between  the  model  and  experiment  for  both  magnitude  and  phase. 


of  the  model  in  predicting  the  first  four  flexible  modes.  As  in  the  45  degree  configuration, 
the  elbow  motor  couples  strongly  into  all  the  system  modes. 
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Elbow 


Figure  3.8:  Elbow-Joint  Frequency  Response:  45  Degree  Elbow  Angle 

For  a  45  degree  elbow  angle,  the  models  predicts  accurately  the  low-frequency  can¬ 
tilevered  zero,  and  the  first,  third,  and  fourth  system  modes.  The  magnitude  plot 
contains  significant  sets  of  double  peaks,  implying  that  the  elbow  motor  couples 
strongly  into  all  the  system  modes. 


3.3,  EXAMPLE:  MODELLING  THE  EXPERIMENTAL  APPARATUS 


Frequency  (Hz) 


Figure  3.9:  Shoulder-Joint  Frequency  Response:  90  Degree  Elbow  Angle 
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Observations 

Small  Variation  with  Configuration  Figure  3.11  shows  a  comparison  of  experimental 
magnitude  plots  for  a  45  and  90  degree  elbow  angle.  This  figure  indicates  that  the  dynamics 


Shoulder 


Figure  3.11:  Comparison  Between  a  45  and  90  Degree  Configuration 

This  hgure  shows  a  comparison  of  experimental  magnitude  plots  for  a  45  and  90 
degree  elbow  angle.  It  is  clear  that  the  Bexible  dynamics  of  the  manipulator  vary 
only  slightly  with  configuration.  As  a  result,  a  single  linear  model  can  be  used  to 
represent  accurately  the  system  for  a  wide  range  of  configurations. 


of  the  system  vary  only  slightly  with  configuration.  As  a  result,  a  single  model  can  be  used 
to  represent  accurately  the  system  for  a  wide  range  of  configurations.  This  is  important  in 
terms  of  enabling  a  simplified  control  design.  Specifically,  a  single  controller  designed  for 
a  nominal  elbow  angle  can  be  used  to  control  the  manipulator,  and  its  fiexible  dynamics, 
throughout  a  large  portion  of  its  workspace. 

Decoupled  Link  Dynamics  Comparison  of  main-arm  models  for  various  configurations 
enabled  some  interesting  observations.  First  of  all,  as  verified  by  the  experimental  plots  in 
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Figure  3.11,  it  was  observed  that  the  natural  frequencies  of  the  main  arm  are  essentially 
constant  over  a  wide  range  of  elbow  angles.  Second,  the  corresponding  system  mode  shapes 
are  qualitatively  very  similar.  Figure  3.12  shows  comparisons  of  the  first  two  theoretical 
flexible  modes  for  90  degree  and  0  degree  elbow-angle  models.  For  each  configuration,  the 
first  mode  is  primarily  an  excitation  of  the  second  link,  and  the  second  mode  is  primarily 
an  excitation  of  the  first  link.  This  characteristic  is  independent  of  configuration.  The 
physical  explanation  of  this  phenomenon  is  that  at  high-frequencies,  there  is  very  little 
energy  exchange  between  the  two  link  subsystems,  due  primarily  to  the  large  mass  and 
inertia  of  the  elbow  motor.  This  low  energy  exchange  implies  that  the  link  dynamics  are 
essentially  decoupled  from  each  other,  and  the  two  subsystems  are  therefore  only  one-way 
coupled  through  the  elbow-motor  control  input.  This  characteristic  was  first  identified  by 
Oakley  [5]. 


Approximate  System  Model 

These  physical  characteristics  were  exploited  to  develop  a  simplified  model  of  the  experi¬ 
mental  system  for  a  90  degree  elbow  angle.  Specifically,  for  an  elbow  angle  of  90  degrees, 
the  system  behaves  like  two  subsystems  that  are  one-way  coupled  through  the  elbow  control 
input.  The  approximate  system  is  shown  in  Figure  3.13.  The  first  subsystem  represents  a 
pinned  flexible  beam  with  an  effective  tip  mass  to  account  for  the  mass  of  the  second-link 
subsystem.  The  second  subsystem  is  a  pinned  flexible  beam  with  an  effective  tip  mass 
and  inertia  representing  the  mini-manipulator  subsystem.  The  two  subsystems  are  one-way 
coupled  through  the  elbow  motor.  Because  the  high-frequency  modes  for  the  individual 
links  vary  only  slightly  with  configuration,  this  approximate  system  model  can  be  used  to 
design  a  controller  which  operates  over  a  wide  range  of  elbow  angles.  The  system  matrices 
for  this  main-arm  model  are  given  in  Appendix  F. 

To  verify  the  accuracy  of  the  approximate  main-arm  model,  comparisons  were  made 
with  the  experimental  apparatus.  Figure  3.14  shows  a  comparison  of  the  collocated  transfer 
functions  from  torque  to  motor  angle  for  both  the  shoulder  and  elbow  motors  for  a  ninety 
degree  elbow  angle.  In  this  model,  some  parameters  were  modified  slightly  to  obtain  a 
better  fit.  Model  accuracy  of  the  first  two  system  modes,  which  dominate  the  motion  of  the 
system,  will  enable  high-performance  control  design  for  this  system. 
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Physical  Characteristics  I 

•  small  variation  in  frequency  with  configuration  | 

•  link  dynamics  are  approximately  decoupled  1 

•  system  is  predominantly  one-way  coupled  1 


Figure  3.12:  Theoretical  System  Modes 

This  figure  illustrates  the  first  two  theoretical  fiexible  modes  shapes  for  two  config¬ 
urations.  The  small  variation  in  natural  frequency  and  corresponding  mode  shapes 
for  the  fiexible  modes  indicates  that  the  link  dynamics  are  essentially  decoupled  for 
a  wide  range  of  elbow  angles.  Thus,  the  two  subsystems  are  only  one-way  coupled 
through  the  elbow-motor  control  input.  This  characteristic,  first  identified  by  Oak¬ 
ley,  can  be  exploited  to  develop  an  approximate  system  model  that  is  accurate  over 
a  wide  range  of  elbow  angles. 
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Figure  3.13;  Approximate  Main- Arm  Model 

Because  the  high-frequency  link  dynamics  are  approximately  decoupled,  the  two- 
link  flexible  manipulator  can  be  well  approximated  as  two  subsystems  that  are 
one-way  coupled  through  the  elbow  control  input.  The  first  subsystem  represents 
a  pinned  Bexible  beam  with  an  effective  tip  mass  to  account  for  the  mass  of  the 
second-link  subsystem.  The  second  subsystem  is  a  pinned  flexible  beam  with  an 
effective  tip  mass  and  inertia  representing  the  mini-manipulator  subsystem.  The 
two  subsystems  are  one-way  coupled  through  the  elbow  motor.  The  resulting  model 
form  will  be  exploited  in  Chapter  5  in  the  control- design  process. 


3.3.5  Total-System  Model 


With  the  accurate  main-arm  model  developed  in  the  last  section,  a  total-system  model  is 
obtained  by  appending  the  model  of  the  mini-manipulator  tip  dynamics.  Specifically,  the 
total-system  dynamics  can  be  represented  by 


Xl 
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0 

0 

_ 1 

Xl 

Bii  Bi2  0 

ui 
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0  A22  0 

X2 

d- 
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U2 

X3 
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X3 

0  0  B33 

U3 

(3.63) 


where  xi  and  X2  represent  the  dynamics  of  the  main  arm  one-way  coupled  through  the 
elbow  control  U2,  and  X3  represents  the  decoupled  tip  dynamics  of  the  mini-manipulator. 


Theta/Torque  (rad/N-m)  Theta/Torque  (rad/N-m) 
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Shoulder 


Frequency  (Hz) 


Figure  3.14:  Modelling  Results 

This  hgure  shows  excellent  correspondence  between  the  approximate  main-arm 
model  with  the  experimental  apparatus  for  a  ninety  degree  elbow  angle.  These 
plots  verify  the  approximations  that  led  to  modelling  the  main  arm  as  two  subsys¬ 
tems  one-way  coupled  through  a  control  input. 
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3.4  Summary 

This  chapter  has  presented  a  modular  approach  to  modelling  multi-link  flexible  manipu¬ 
lators.  This  approach  is  based  on  a  new  model  concatenation  technique  that  enables  the 
concatenation  of  two  state-space  models  that  contain,  as  inputs,  the  constraint  forces  that 
physically  couple  the  subsystems.  By  judiciously  solving  for  the  constraint  forces,  a  total- 
system  model  of  the  coupled  subsystems  is  attained. 

The  modularity  of  this  approach  leads  to  a  number  of  advantages  in  terms  of  modelling 
multi-link  flexible  manipulators.  First,  it  enables  modelling  complex  systems  to  be  parti¬ 
tioned  into  smaller  more  tractable  modelling  problems.  A  second  advantage  is  that  sub¬ 
system  modifications,  substitutions,  and  additions  can  be  easily  accommodated,  without 
rederiving  equations  of  motion  for  the  unaffected  subsystems.  Furthermore,  this  approach 
is  amenable  to  modelling  systems  in  a  number  of  configurations. 

This  modnlar  approach  has  been  applied  to  the  experimental  apparatus.  The  approach 
involved  modelling  independently  the  two  flexible  links,  and  then  concatenating  these  sub¬ 
system  models  to  obtain  a  model  of  the  flexible  main  arm.  This  was  performed  for  various 
configurations  and  the  corresponding  models  were  verified  by  comparisons  with  the  experi¬ 
mental  system.  A  total-system  model  of  the  flexible  main  arm/mini-manipulator  system  was 
developed  by  concatenating  the  flexible-arm  model  with  a  model  of  the  mini-manipulator 
tip  dynamics. 

In  Chapters  4  and  5,  the  model  developed  in  this  chapter  will  be  used  for  trajectory 
generation  and  control  design  for  the  experimental  apparatus. 


Chapter  4 


Trajectory  Generation 


This  chapter  describes  a  trajectory-generation  approach  for  repositioning  quickly  multi-link 
flexible  manipulators,  and  presents  an  application  of  this  approach  to  the  experimental  ap¬ 
paratus.  Given  the  reference  commands  as  inputs,  this  technique  generates  optimal  desired 
state  histories  and  the  corresponding  feedforward-control  inputs  to  the  plant’s  actuators  for 
the  repositioning.  Figure  4.1  illustrates  how  the  resulting  trajectory  generator  fits  into  the 
solution  framework  described  in  Chapter  1. 
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Figure  4.1:  Control  Framework 

This  chapter  describes  one  component  of  the  control-system  framework:  the 
optimal-trajectory  generator.  Taking  the  reference  commands  as  inputs,  the  tra¬ 
jectory  generator  computes  the  desired  plant-state  history  and  the  corresponding 
feedforward-control  input  to  the  plant’s  actuators  to  effect  this  desired  motion. 


The  goal  of  this  research  is  to  achieve  high-performance  point-to-point  repositioning  of 
multi-link  flexible  manipulators  with  local  end-point  manipulation.  In  the  repositioning, 
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only  the  slew  time  and  the  final  tip  position  of  the  manipulator  are  specified.  The  control 
strategy  developed  to  meet  this  goal  requires  transporting  quickly  the  mini-manipulator  to 
within  a  target  area,  so  that  the  desired  target  point  is  within  its  reach.  The  corresponding 
main-arm  performance  objective  requires  repositioning  quickly  the  main  arm,  and  thereafter 
maintaining  the  mini-manipulator  within  the  target  area.  The  requirement  of  the  trajectory 
generator  is  to  provide  a  desired  state  history  and  feedforward-control  input  to  the  main 
arm  to  position  quickly  the  mini-manipulator  near  the  target. 

There  are  two  characteristics  of  the  point-to-point  repositioning  problem  addressed  in 
this  research  that  are  exploited  to  enable  a  new  trajectory-generation  approach.  First, 
because  of  the  redundancy  introduced  by  the  mini-manipulator,  the  main  arm  does  not  need 
to  meet  exactly  a  specific  terminal  condition.  Instead,  the  main  arm  must  be  positioned 
rapidly  to  any  location  within  a  very  specific  area  near  the  desired  end-point  target.  Second, 
because  only  the  final  tip  position  is  specified,  the  main  arm  is  not  required  to  follow  a 
previously  specified  path  during  the  repositioning. 

This  trajectory-generation  approach,  based  on  a  model-following  implementation  of  a 
terminal  controller,  exploits  these  characteristics.  Consequently,  this  approach  offers  bene¬ 
fits  in  terms  of  performance,  simplicity,  and  computational  cost  : 

®  First,  the  main  arm  is  not  required  to  follow  a  previously  specified  path  during  the 
repositioning,  and  is  therefore  able  to  follow  a  “best”  path.  Because  the  terminal  area 
to  be  reached  is  of  finite  size,  there  are  a  number  of  acceptable  terminal  conditions  and 
corresponding  paths  that  the  arm  can  follow.  Consequently,  this  approach  generates, 
in  real  time^  an  optimal  trajectory  for  the  main  arm.  ^  Included  in  this  trajectory  is 
the  optimal  state  history,  and  the  corresponding  optimal  control  input. 

•  Second,  because  the  only  slew-dependent  off-line  computations  of  this  approach  are 
based  on  the  specified  slew  time,  repositionings  of  the  same  slew  time  require  no 
additional  ojj-line  computations, 

®  Third,  the  precomputations  of  this  approach  are  computationally  ejficient.  This  ef¬ 
ficiency,  enabled  by  the  soft  terminal  constraints  in  the  terminal-controller  design, 
offers  implementation  and  design  benefits. 

These  three  characteristics  are  described  in  detail  in  this  chapter. 


# 


^Optimality  is  defined  by  a  linear- quadratic  cost  function.  This  is  explained  in  Section  4.2.1. 
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The  reference  commands  for  a  point-to-point  repositioning  consist  of  the  final  tip  position 
and  the  slew  time.  Generating  a  trajectory  based  on  these  commands  requires  computing  a 
desired  state  history  for  the  repositioning  as  well  as  the  corresponding  feedforward-control 
input  that,  in  the  presence  of  an  exact  model,  causes  the  desired  motion. 

Typically,  trajectory  generation  for  point-to-point  repositionings  involves  two  sets  of 
off-line  computations.  Figure  4.2  illustrates  a  typical  trajectory-generation  framework.  The 
trajectory  generator  takes  as  input  the  desired  reference  commands.  Within  the  trajectory 
generator,  an  off-line  calculation  is  performed  to  compute  a  desired  plant-state  history  from 
the  reference  commands.  From  this  desired  output,  another  off-line  calculation  yields  the 
corresponding  feedforward-control  input  to  the  plant’s  actuators. 


Feedforward  Input 
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Commands 


Trajectory  Generator 


Desired  State 


Figure  4.2:  Typical  Trajectory-Generation  FrameAvork 

The  trajectory  generator  takes  as  input  the  desired  reference  commands.  Within 
a  typical  trajectory  generator,  an  off-line  calculation  is  performed  to  compute  a 
desired  plant-state  history  from  the  reference  commands.  From  this  desired  output, 
another  off-line  computation  generates  the  corresponding  feedforward-control  input. 


Recent  research  has  addressed  the  inverse-dynamics  problem  for  flexible  manipulators 
to  calculate  the  exact  control  inputs  to  follow  a  specified  manipulator  end-point  path.  A 
good  representation  of  this  research  can  be  found  in  [15],  [16],  and  [17].  These  methods 
have  been  verified  experimentally  and  demonstrated  to  work  quite  well. 
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Furthermore,  numerous  methods  exist  for  taking  the  reference  commands  of  a  point- 
to-point  repositioning  and  generating  a  desired  plant  output.  The  most  common  method 
involves  fitting  a  spline  between  the  initial  and  final  tip  positions,  and  then  computing  de¬ 
sired  joint  angles  through  inverse  kinematics  assuming  rigid  links.  This  method,  however, 
is  sub-optimal  for  flexible  manipulators  due  to  the  rigid-links  assumption.  Another  method 
computes  desired  end-point  histories  for  point-to-point  motions  with  minimal  residual  vi¬ 
bration  by  setting  the  end-point  acceleration  and  jerk  to  zero  (  [42]  and  [43]).  In  addition, 
input-shaping  techniques  can  be  applied  to  generate  a  desired  output  path  resulting  in 
reduced  residual  vibrations  (  [44],  [20],  [21],  [22],  [23],  [45],  and  [19]). 


4.2  Trajectory-Generation  Approach 

In  contrast  to  typical  trajectory-generation  approaches,  the  approach  developed  in  this 
dissertation  generates  in  real  time  an  optimal  output  for  the  repositioning,  and  generates 
simultaneously  the  corresponding  control  input.  This  approach  is  based  on  the  model¬ 
following  control  framew^ork  shown  in  Figure  4.3  in  which  a  plant  model  is  regulated  by 
a  terminal  controller.  Although  the  structure  of  this  framework  is  not  new,  the  novelty 
stems  from  the  model-following  implementation  of  the  terminal  controller. 

Rather  than  calculate  off-line  the  desired  plant  output  and  then  the  corresponding  con¬ 
trol  inputs,  the  terminal  controller  regulates  in  real  time  the  plant  model  to  the  desired  final 
position.  The  output  of  the  terminal  controller  is  also  sent  to  the  plant  as  a  feedforward- 
control  input. ^  The  corresponding  response  of  the  plant  model,  assuming  the  model  is 
initialized  with  the  initial  conditions  of  the  plant,  represents  the  desired  state  history  for 
the  plant  to  follow. 

Given  this  model-following  framework,  successful  trajectory  generation  places  specific 
requirements  on  the  terminal-controller  design.  This  section  presents  the  terminal-controller 
problem  formulation  and  solution,  the  specific  requirements  of  the  design,  and  the  approach 
developed  to  meet  these  requirements. 


^Because  of  this  model-following  implementation,  the  terminal  controller  has  no  effect  on  the  closed-loop 
stability  of  the  actual  plant. 
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Figure  4.3:  Terminal- Controller-Based  Trajectory  Generator 

The  terminal-controller-based  trajectory-generation  framework  consists  of  a  plant 
model  regulated  by  a  terminal  controller.  The  output  of  the  terminal  controller  is 
sent  to  the  plant  as  a  feedforward-control  input.  The  corresponding  response  of  the 
plant  model  to  this  same  input  represents  the  desired  state  history  for  the  plant  to 
follow  during  the  repositioning. 


4.2.1  Problem  Formulation  and  Solution 

Suitable  trajectory  generation  reduces  to  designing  a  terminal  controller  that  meets  the 
main-arm  requirements  for  a  point-to-point  repositioning.  Specifically,  the  goal  is  to  bring 
the  main  arm  close  to  a  desired  final  state  at  a  specified  final  time,  while  at  the  same  time 
maintaining  reasonable  values  of  the  state  and  not  exceeding  acceptable  levels  of  control 
effort. 

An  intuitive  way  of  achieving  these  performance  objectives  is  to  formulate  a  linear- 
quadratic  performance  index  that  weights  the  quantities  of  interest,  and  then  minimize  the 
value  of  this  cost  function  with  respect  to  the  control  effort. 

As  described  in  Chapter  3,  the  main-arm  dynamics  are  represented  accurately  by 

x(^)  =  Ax(^)  +  Bu(t),  (4.1) 

where  x(t)  is  an  n-component  vector  of  state  elements,  u{t)  is  an  m-component  vector 
of  control  inputs,  and  A  and  B  are  constant  system  matrices.  Control  design  involves 
minimizing  the  cost  function 


3 


(4.2) 
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with  respect  to  the  control  input  u(t),  and  subject  to  the  linear  constraints  imposed  by 
Equation  (4.1).  Sf  and  Qa;(t)  are  positive  semidefinite  matrices  that  weight  the  values  of 
the  states  at  the  terminal  time  x(tf)  and  the  values  of  the  intermediate  states  x(t).  Qu(t) 
is  a  positive  definite  matrix  that  weights  the  control  effort  u(t)  expended  during  the  slew. 
Equation  (4.2)  corresponds  to  a  regulator  design  where  the  desired  terminal  state  is  zero. 
As  stated,  this  problem  formulation  is  known  as  a  terminal- controller  problem  [46],  and  its 
solution  is  well  known. 

Because  of  the  soft  terminal  constraints  on  the  main  arm,  an  acceptable  terminal- 
controller  design  can  be  determined  using  a  finite  weighting  on  the  terminal  state.  This 
finite  value  of  S/  enables  a  straightforward  computationally  efficient  solution.  Specifically, 
using  methods  described  in  [46],  the  optimal  control  input  u(t)  is  found  by  minimizing 
Equation  (4.2)  while  solving  simultaneously  the  linear  constraints  in  Equation  (4.1).  The 
details  of  this  solution  and  the  corresponding  discrete  formulation  and  solution  can  be  found 
in  Appendix  D.  The  optimal  feedback  law  is  given  as: 

u(t)  =  -K(t)x(t),  (4.3) 

K{t)  =  Q„(t)-iB^S(t),  (4.4) 

where  K(t)  is  the  optimally  determined  time- varying  feedback  gain,  and  S(t)  is  defined  by 
the  matrix  Riccati  equation 

S  =  -SA- A'^S  +  SEQ-^B^S-Qu.  (4.5) 

Equation  (4.5)  can  be  integrated  backward  from  the  terminal  time  t_f  to  the  initial  time  to 
to  solve  for  S(t).  Once  S{t)  has  been  found,  the  continuous  control  law  and  time- varying 
feedback  gains  can  be  calculated. 

There  are  two  characteristics  of  the  terminal-controller  solution  that  are  advantageous 
in  terms  of  trajectory  generation: 

•  First,  because  the  terminal  time  is  the  only  slew  parameter  on  which  the  solution 
depends,  it  is  not  necessary  to  redesign  the  controller  for  every  set  of  initial  and  final 
slew  configurations.  Rather,  a  redesign  is  needed  only  for  slews  of  different  final  times. 

•  Second,  because  the  terminal-controller  solution  is  a  backward  sweep  and  does  not 
involve  iteration,  computation  of  the  solution  demands  minimal  processor  time.  This 
computationally  efficient  solution  is  a  result  of  the  soft  terminal  constraints  on  the 
main  arm  enabled  by  the  redundancy  of  the  mini-manipulator. 
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Design  Strategy  There  are  a  number  of  terminal-controller-design  strategies  that  can  be 
adopted.  One  approach  involves  designing  at  the  beginning  of  the  slew  a  new  set  of  terminal- 
controller  gains  for  each  new  terminal  time.  This  approach  results  in  a  computational  delay 
prior  to  carrying  out  the  slew  to  perform  the  backward  sweep.  An  alternative  approach 
adopted  in  this  research  computes  ahead  of  time  and  stores  the  controller  gains  for  the 
repositioning  times  of  interest.  As  a  result,  no  subsequent  off-line  computations  are  required, 
and  no  delay  is  experienced  at  the  beginning  of  the  slew. 

4.2.2  Benefits 

The  benefits  of  this  approach  stem  from  the  fact  that  the  trajectory  generation  is  based  on 
a  terminal-controller  design.  Specifically, 

•  For  a  given  performance  index,  the  terminal  controller  is  designed  to  reposition  opti¬ 
mally  the  plant  model.  Thus,  this  trajectory-generation  approach  provides  the  plant 
with  an  optimal  trajectory  for  the  point-to-point  repositioning. 

•  This  approach  exploits  the  redundancy  introduced  by  the  mini-manipulator.  This  idea 
is  incorporated  into  the  trajectory  generation  in  the  form  of  soft  terminal  constraints 
in  the  terminal  controller  design.  The  soft  terminal  constraints,  rather  than  hard 
constraints,  allow  for  the  straightforward,  computationally  efficient  terminal-controller 
solution. 

•  The  slew  time  is  the  only  slew-dependent  parameter  in  the  terminal-controller  design. 
As  a  result,  for  repositionings  of  the  same  slew  time,  no  additional  precomputations 
are  required. 

4.2.3  Trajectory-Generation  Requirements 

This  trajectory-generation  approach  offers  benefits  in  terms  of  performance,  and  implemen¬ 
tation  and  design  simplicity.  However,  along  with  these  advantages  come  three  requirements 
of  the  resulting  trajectory  to  achieve  acceptable  performance.  First,  as  described  in  Chapter 
1,  the  system  will  be  in  a  regulator  mode  after  the  final  time  tf.  Consequently,  after  t/,  the 
desired  state  provided  by  the  trajectory  generator  must  correspond  to  the  desired  tip  posi¬ 
tion.  Second,  at  t  =  a  control-mode  transition  occurs  from  sending  both  feedforward  and 
feedback  inputs  to  the  plant,  to  feedback  alone.  The  terminal  controller  must  be  designed 
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so  that  this  transition  is  smooth.  Finally,  the  design  of  the  terminal  controller  is  based  on 
a  model  that  is  accurate  over  a  limited  frequency  range.  Therefore,  the  terminal-controller 
design  must  be  robust  to  unmodelled  dynamics. 

Desired  State  After  tj 

The  terminal  controller  is  designed  to  bring  the  plant  model  close  to  the  reference  commands 
by  the  final  time.  Consequently,  the  model  state  will  not  match  exactly  the  reference 
state  at  t  =  tf.  Since  the  main  arm  should  be  regulated  to  the  reference  commands  for 
t  >  tf,  ii  is  necessary  for  the  model  state  to  correspond  to  the  reference  commands  once 
the  repositioning  is  complete. 

The  approach  adopted  to  address  this  issue  involves  regulating  the  model  state  to  the 
reference  state  for  t  >  t/.  At  t  =  tf,  &  constant-gain  regulator  regulates  the  model  state  to 
the  reference  state,  although  the  output  of  this  controller  is  not  sent  to  the  actual  plant.  To 
ensure  that  the  response  of  the  model  is  smooth,  the  constant-gain  controller  is  designed  to 
be  frequency  limited  to  not  excite  high-frequency  dynamics  in  the  model.  ^ 

Smooth  Transition  at  t  =  tf 

As  described  in  Chapter  1,  the  solution  framework  developed  in  this  research  sums  the 
feedforward-control  input  from  the  trajectory  generator,  u/,  and  the  feedback-control  input, 
Ua,  to  force  the  plant  to  follow  the  desired  plant  state  in  the  presence  of  disturbances  and 
other  unmodelled  effects.  At  t  —  tf,  the  control  input  to  the  plant  switches  from  the  sum 
of  u/  and  Uq  to  just  Uo.  If  the  final  value  of  the  feedforward  input  u/(t/)  is  non-zero, 
there  will  be  a  discontinuity  in  the  control  sent  to  the  plant.  This  discontinuity,  if  large, 
will  result  in  a  non-smooth  control-mode  transition.  Consequently,  an  essentially  bumpless 
control-mode  transfer  requires  that  u/(t/)  ~  0. 

Designing  a  terminal  controller  such  that  u/(t/)  w  0  can  be  achieved  by  time  weighting 
the  terminal-controller  cost  function.  Specifically,  by  weighting  heavily  the  amount  of  con¬ 
trol  effort  used  near  the  end  of  the  slew,  the  output  of  the  terminal  controller  will  be  small 
near  the  final  time.  This  involves  selecting  Qu{t)  to  be  large  for  values  of  t  close  to  t  =  tf. 

In  addition  to  enabling  a  smooth  transition  at  the  end  of  the  slew,  time  weighting  the 
terminal-controller  cost  function  is  consistent  with,  and  helps  achieve,  the  first  research  task 


^Chapter  5  provides  details  on  designing  a  frequency-shaped  regulator. 
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described  in  Chapter  1:  to  develop  a  main-arm  control  approach  for  positioning  quickly  the 
mini-manipulator  within  the  desired  workspace.  Time  weighting  the  cost  function  causes 
the  terminal  controller  to  perform  most  of  the  trajectory  well  before  the  final  time,  while 
the  “cost”  of  control  is  low.  As  a  result,  the  main  arm  transports  the  mini-manipulator  to 
the  desired  target  area  faster. 


Robustness  to  High-Frequency  Unmodelled  Dynamics 

For  flexible  manipulators  with  unmodelled  high-frequency  modes,  it  is  desirable  to  shape 
the  frequency  content  of  the  trajectories  to  not  excite  unmodelled  lightly-damped  flexible 
modes.  This  can  be  achieved  through  a  frequency-shaped  design  (described  for  the  standard 
LQG  design  in  Chapter  5)  which  corresponds  to  introducing  “virtual  actuator  dynamics” 
or  actuator  prefilters  [47]. 

Incorporating  actuator  prefilters  into  the  terminal-controller  design  and  implementation 
attenuates  high-frequency  inputs  sent  to  the  plant.  The  prefilter  implementation  is  shown 
in  Figure  4.4.  Implementation  of  the  actuator  prefilters  must  be  accounted  for  in  the 


U 


Plant 

Output 


Figure  4.4:  Actuator  Prefiltering 

Incorporating  actuator  prefilters  into  the  terminal- controller  design  provides  a 
method  for  achieving  a  frequency-shaped  design.  Actuator  prefilters  Alter  the  con¬ 
trol  input  to  attenuate  high-frequency  inputs  that  would  excite  high-frequency  un¬ 
modelled  modes  of  the  plant. 


terminal-controller  design.  This  is  done  by  augmenting  the  plant  state  with  the  prefilter 
state,  X/.  Specifically,  the  performance  index  of  Equation  (4.2)  must  be  modified  to 


J.  - 


(4.6) 
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where  x*  is  defined  as 


(4.7) 


and  S/^,  and  Qu,(t)  are  weighting  matrices  for  the  augmented  system  defined  by 

x^,.  The  actuator  prefilter  state  is  defined  by  the  filter  state-space  model 


X/  =  A/X/-I-B/U  (4.8) 

u/  =  C/x/-t-D/u  (4.9) 


where  the  output  of  the  filter,  u/,  is  the  input  to  the  physical  plant.  The  terminal-controller 
design  involves  minimizing  Equation  (4.6)  subject  to  the  constraints 

Xs  =  A^x^-l-BsU  (4-10) 


where 


A. 

B. 


A  BCf 
0  Af 

BBf 

B/ 


The  resulting  terminal  controller  is  of  the  form 


u(t)  =  -Ks{t)Xs{t) 


(4.11) 

(4.12) 


(4.13) 


where 

Ksit)  =  [  K(t)  Kf{t)  ]  (4.14) 

and  K/(t)  is  a  feedback  gain  on  the  prefilter  state. 

Equation  (4.14)  indicates  that  implementation  of  the  frequency-shaped  terminal-control¬ 
ler  design  includes  a  feedback  term  on  the  prefilter  state.  Figure  4.5  illustrates  this  imple¬ 
mentation. 


4.2.4  Design  Approach 

Designing  a  terminal  controller  requires  selecting  appropriately  the  weighting  matrices 
Qi(^),  Quit),  and  S/  to  achieve  desired  performance  most  closely.  For  a  system  with  n 
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Controller 


Figure  4.5:  Frequency-Shaped  Terminal-Controller  Implementation 

Implementing  the  frequency-shaped  terminal  controller  requires  filtering  u  to  gen¬ 
erate  the  actual  control  input  Uf,  as  well  as  including  an  additional  feedback  term 
based  on  the  prefilter  state. 


states  and  m  inputs,  specifying  the  weighting  matrices  requires  selecting  elements  of 
Qx{t)  and  nf  elements  of  Qu(t)  for  0  <  t  <  tf,  and  the  v?  elements  of  S/.  For  a  final  time 
tf  and  a  sample  period  Tg,  Qu(t)  and  Qx(t)  must  be  specified  for  ^  time  steps. 

Although  Qu(t)  must  be  varied  with  time  to  meet  the  trajectory  requirements,  selection 
of  the  weighting  matrices  can  be  simplified  by  choosing  Qx(t)  to  be  constant  for  0  <  t  <  f/. 
In  addition,  the  selection  can  be  simplified  further  by  regulating  the  outputs  in  a  coordinated 
fashion.  If  the  outputs  to  be  controlled,  yc  6  IR”°,  can  be  expressed  as 


Yc 

=  CcX, 

(4.15) 

then  the  weighting  matrices  can  be  selected  as 

S/ 

=  C/W,Cc 

(4.16) 

Qx 

=  CjWxCc 

(4.17) 

Quit) 

=  p(<)w„ 

(4.18) 

where  6  and  Wx  €  are  diagonal  matrices  that  weight  the  terminal  and 

intermediate  values  of  the  controlled  outputs,  G  is  a  constant  diagonal  matrix, 

and  p(t)  is  a  scalar  that  varies  the  weighting  on  the  control  input  as  a  function  of  time. 
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Selecting  the  weighting  matrices  in  this  way  reduces  the  design  to  choosing  Uo  parameters 
for  both  W5  and  W^;,  ni  elements  of  Wu,  and  the  scalar  function  p{t). 

The  corresponding  frequency-limited  trajectories  can  be  found  by  augmenting  the  model 
system  and  output  matrices  accordingly,  and  leaving  the  design  parameters  unchanged  such 
that 


where 


S/3  -  c7w,c, 

Qxs  =  CjW.Cs 
QuM)  =  p{t)yVu 


c,= 


and  Uf.  corresponds  to  the  order  of  the  prefilter  for  the  actuator. 


(4.19) 

(4.20) 

(4.21) 


(4.22) 


4.3  Design  Example;  The  Flexible  Main  Arm 

The  implementation  approach  adopted  in  this  research  is  to  design  and  store  terminal- 
controller  feedback  gains  for  various  values  of  t/  so  that  the  user  can  simply  select  one  of  the 
predetermined  final  times  for  each  repositioning.  In  this  way,  once  the  designs  are  complete, 
no  additional  off-line  precomputations  are  required.  This  section  illustrates  the  terminal- 
controller  design  process  for  the  experimental  main  arm  for  a  two-second  repositioning,  and 
then  presents  example  trajectories  for  other  final  times. 


4.3.1  Terminal-Controller  Design  for  tf  =  2  Seconds 

Using  the  linear  model  developed  in  Chapter  3  of  the  main  arm  for  a  ninety  degree  nominal 
elbow  angle,  the  controlled  outputs  yc,  defined  in  Figure  4.6,  are  given  by 
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/X 


Figure  4.6:  Controlled  Outputs 

This  figure  iUnstrates  the  controlled  outputs  of  the  manipulator,  wupi  and  dupi 
represent  the  lateral  defection  and  rotation  of  the  tip  of  link  i.  The  inertial  coordi¬ 
nate  frame  X,y,  0  is  located  at  the  shoulder  joint  and  fixed  in  inertial  space.  Also 
shown  are  the  shoulder  and  elbow  motor  inputs  ui  and  it2- 


Yc  = 


'^tip2 

^tip2 

^tipl 

'^tip2 

'^tipl 

dtip2 

^tipl 


Ccx 


(4.23) 


where  wupi  and  Oupi  represent  the  lateral  deflection  and  rotation  of  the  tip  of  link  i.  The 
inertial  coordinate  frame  X,  Y",  0  is  located  at  the  shoulder  joint  and  fixed  in  inertial  space. 
The  design  parameters  were  initially  chosen  as 


W, 

=  1500  X  Ig 

(4.24) 

=  Is 

(4.25) 

=  l2 

(4.26) 
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p(t)  =  1  (4.27) 

where  Ij  is  the  j  x  j  identity  matrix.  Physically,  these  design  parameters  weight  heavily 
the  terminal  error  of  the  controlled  outputs  to  insure  that  the  main  arm  brings  the  mini¬ 
manipulator  close  to  the  desired  target  very  quickly.  The  corresponding  feedback  gains  are 
shown  in  Figure  4.7.  In  general,  the  gains  are  relatively  small  at  the  beginning  of  the 


Figure  4.7:  Initial  Terminal-Controller  Gains 

This  figure  shows  the  terminal- controller  gains  as  a  function  of  time.  The  gains 
tend  to  increase  near  the  final  time  to  make  the  terminal  errors  small. 


slew,  and  then  increase  near  the  end  of  the  slew  to  make  the  terminal  errors  small  quickly. 
Rather  than  plot  all  elements  of  the  desired  state  history  for  a  repositioning  in  which  the 
tip  is  commanded  to  move  10  cm  in  both  the  X  and  Y  directions,  the  corresponding  desired 
tip  trajectory  and’ the  corresponding  feedforward  control  are  shown  in  Figure  4.8. 

Although  the  tip  trajectory  is  smooth,  this  design  is  unacceptable  because  of  the  high- 
frequency  content  of  the  torque  profiles  at  the  beginning  and  end  of  the  slews.  It  is  desirable 
to  smooth  the  torque  profiles  so  that  the  feedforward  input  does  not  excite  unmodelled 
lightly-damped  high-frequency  modes  in  the  arm. 

To  limit  the  frequency  content  of  the  feedforward  input,  the  model  is  augmented  with 
actuator  prefilters  as  described  in  the  previous  section.  The  prefilters  for  both  the  shoulder 
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Figure  4.8:  Initial  Trajectory 

This  figure  displays  the  desired  tip  response  that  corresponds  to  the  generated 
desired  state  history,  as  well  as  the  control  inputs  that  are  generated  by  the  feedback 
gains  of  Figure  4. 7.  The  dashed  lines  in  the  tip  response  indicate  the  desired  terminal 
values.  The  high-frequency  content  at  the  beginning  and  end  of  the  torque  profiles 
is  undesirable  when  lightly-damped  unmodelled  modes  are  present.  Consequently, 
the  trajectory  displayed  in  this  figure  motivates  the  need  for  a  frequency-shaped 
design. 


and  elbow  motors  are  fifth-order  Butterworth  filters  with  a  5  Hz  cut-off  frequency.  The 
resulting  frequency-limited  trajectory  is  given  in  Figure  4.9. 

To  achieve  a  smooth  control-mode  transfer,  it  is  necessary  to  modify  the  design  so 
that  n{tf)  ^  0.  Consequently,  it  is  desirable  to  select  a  scalar  function  p{t)  such  that  the 
weighting  on  the  control  Q-u(t)  becomes  larger  as  t  approaches  tj.  Figure  4.10  shows  the 
p{t)  that  was  selected  in  this  design.  The  physical  interpretation  of  this  function  is  that  the 
cost  of  control  is  low  and  constant  for  t  <  O.Stj,  and  then  becomes  increasingly  high  for 
t  >  O.Stf.  For  this  design,  p{t)  ~  1  for  t  <  0.8t/,  and  for  t  >  O.Stf 

f(()=(^-0.8^+l) 


where  Tg  =  ^sec. 


(4.28) 
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Figure  4.9:  Frequency-Shaped  Trajectory 

This  figure  shows  the  feedforward-control  input  and  the  desired  tip  response  for 
the  frequency-shaped  design.  The  torque  profiles  are  smoother  than  the  profiles  in 
Figure  4.8  due  to  the  introduction  of  actuator  prefilters. 


By  time-weighting  the  cost  function,  the  cost  of  control  is  very  high  near  the  end  of  the 
slew.  The  controller,  therefore,  attempts  to  effect  most  of  the  motion  early  w'hen  control 
is  cheap,  and  thereby  needs  to  make  only  fine  adjustments  during  the  last  part  of  the  slew 
when  control  is  expensive. 

The  final  terminal-controller  trajectory  for  a  two-second  terminal  time  is  given  in  Fig¬ 
ure  4.11.  The  symmetric  torque  profiles  in  Figure  4.11  correspond  to  a  “near-minimum¬ 
time”  response.  The  actuators  “push”  hard  during  the  first  half  of  the  slew,  and  “pull” 
equally  hard  during  the  second  half  of  the  slew  to  bring  the  system  to  rest. 


4.3.2  Additional  Designs 

For  the  same  weighting  matrices,  terminal  controllers  were  designed  for  slew  times  of  1.5, 
2.5,  and  3  seconds  by  simply  changing  the  final  time  in  the  backward  sweep  solution. 
The  corresponding  trajectories  are  displayed  in  Figures  4.12  through  4.14  for  a  specific 
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Figure  4.10:  Scalar  Weighting  Function  p{t) 

This  figure  shows  the  scalar  function  that  makes  the  cost  of  control  a  function  of 
time.  This  particular  function  will  penalize  increasingly  the  control  after  t  —  O.Stf 
to  force  n{tf)  ^  0. 


repositioning  in  which  the  manipulator  tip  is  commanded  to  move  10  cm  in  both  the  X  and 
Y  directions. 
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Figure  4.11:  Trajectory  for  tf  =  2.0  Seconds 


Figure  4.12:  Trajectory  for  tf  =  1.5  Seconds 
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Figure  4.13;  Trajectory  for  tf  =  2.5  Seconds 


Figure  4.14:  Trajectory  for  tf  —  3.0  Seconds 
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4.4  Summary 

This  chapter  described  a  new  trajectory-generation  approach  that  computes  an  optimal 
state  history  and  corresponding  feedforward-control  input  for  point-to-point  repositionings 
of  multi-link  flexible  manipulators  with  a  specified  final  time.  By  formulating  the  reposi¬ 
tioning  problem  as  a  terminal-control  problem,  this  approach  exploits  two  characteristics  of 
the  point-to-point  repositioning  problem  addressed  in  this  research:  (1)  the  soft-terminal- 
control  approach  enabled  by  the  redundancy  introduced  by  the  mini-manipulator,  and  (2) 
the  opportunity  to  follow  an  optimal  path  during  the  repositioning  since  only  the  final  tip 
position  is  specified. 

This  approach  is  based  on  a  model-following  implementation  of  a  terminal  controller 
that  regulates  in  real  time  a  plant  model  to  the  reference  commands.  The  output  of  the 
terminal  controller  is  sent  to  the  plant  as  a  feedforward-control  input.  The  corresponding 
response  of  the  plant  model  represents  the  desired  state  history  for  the  plant  to  follow  during 
the  repositioning. 

A  terminal-controller  design  for  the  main  arm  was  used  to  illustrate  the  design  process. 
The  resulting  feedforward  component  of  the  control  system  will  be  used  -  together  with 
the  feedback  component  ~  in  Chapter  6  to  generate  -  and  execute  -  optimal  trajectories  for 
point-to-point  repositionings  of  the  experimental  system. 


Chapter  5 


Feedback  Control 


This  chapter  describes  the  control  approach  developed  in  this  research  for  multi-link  flexible 
systems,  and  presents  an  application  of  this  approach  to  the  experimental  apparatus.  Fig¬ 
ure  5.1  illustrates  how  the  resulting  feedback-control  component  fits  into  the  control-system 
framework  described  in  Chapter  1.  Feedback  control  combines  with  the  feedforward  input 


Feedforward 

Control 


Desired  _ j. 


Reference 

Plant 

Feedback  ^ 

Commands 

Trajectory 

State 

Feedback 

Control 

si 

Plant 

1  Generator 

! 

CoJiiroJler 

Figure  5.1:  Control  Framework 

Chapter  4  presented  the  trajectory-generation  component  of  the  solution  framework. 
This  chapter  presents  the  feedback- control  approach  developed  in  this  research.  The 
feedback  controller  generates  a  control  input  that  augments  the  feedforward  input  to 
regulate  the  plant  to  the  desired  plant-state  history  in  the  presence  of  disturbances 
and  unmodelled  effects. 


to  regulate  the  plant  to  the  desired  plant-state  history  in  the  presence  of  disturbances  and 
unmodelled  effects. 

The  control  approach  developed  in  this  dissertation  enables  the  development  of  high- 
performance  controllers  for  predominantly  one-way  coupled  system  to  be  partitioned  into 
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sequential  subsystem  designs.  This  sequential  process  exploits  the  one-way  coupling  that 
characterizes  the  system  within  the  frequency  range  of  interest  for  control.  Because  the 
one-way  coupling  assumption  is  inaccurate  outside  of  this  frequency  range,  the  subsystem 
controllers  are  designed  to  be  robust  to  this  model  uncertainty.  As  a  result,  this  approach 
enables  design  for  high-performance  control  of  complex  systems  to  be  partitioned  into  design 
steps  for  smaller,  more  tractable  subsets. 

5.1  Background 

Eric  Schmitz  demonstrated,  on  a  single-link  flexible  manipulator,  a  factor  of  four  improve¬ 
ment  in  control  bandwidth  over  conventional  joint-based  control  through  a  linear-quadratic- 
gaussian  (LQG)  control  strategy  that  incorporated  end-point  measurements  ([4]  and  [6]). 
Similar  performance  improvements  were  demonstrated  by  Celia  Oakley  who  achieved  high- 
performance  end-point  control  of  a  two-link  flexible  manipulator  [5].  This  previous  work 
generated  tw'O  important  findings: 

•  First,  end-point  feedback  is  critical  to  achieving  high-performance  control  of  flexible 
manipulators. 

•  Second,  LQG  theory  provides  a  framework  for  developing  control  systems  that  incor¬ 
porate  effectively  end-point  feedback. 

Based  on  the  results  of  this  previous  work,  an  LQG  framework  incorporating  end-point 
feedback  was  chosen  as  the  basis  of  the  control  approach  developed  in  this  research  for 
predominantly  one-way  coupled  systems. 

Systems  that  are  predominantly  one-way  coupled  can  be  modelled  as  one-way  coupled 
within  the  frequency  range  of  interest  for  control.  Specifically,  within  this  frequency  range, 
the  equations  of  motion  for  a  predominantly  one-way  coupled  system  made  up  of  two  sub¬ 
systems  represented  by  xi(t)  and  X2(t)  are  of  the  form 


xi(Q 

All  Ai2 

xi(Q 

-k 

Bii 

Bi2 

Ui(t) 

- 1 

CM 

C 

o 

0 

B22 

.  . 

The  subsystems  of  Equation  (5.1)  are  one-way  state  and  input  coupled  since  X2(t)  is  inde¬ 
pendent  of  both  xi(t)  and  Ui(t).  Because  Equation  (5.1)  accurately  represents  the  system 
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within  the  frequency  range  of  interest,  it  is  a  suitable  model  for  control  design.  However,  a 
model  of  the  plant  that  is  accurate  for  all  frequencies  is  given  by 


Xi(f) 

’  An 

Ai2 

Xi(f) 

+ 

Bn 

Bi2 

Mi) 

.  _ 

A21 

A22 

.  . 

B21 

B22 

Equation  (5.2)  subsumes  Equation  (5.1)  in  that  the  models  are  identical  within  the  frequency 
range  of  interest,  and  deviate  outside  of  this  frequency  range.  As  a  result,  control  design 
based  on  Equation  (5.1)  must  be  made  robust  to  model  uncertainty  for  the  frequency  ranges 
where  the  model  is  inaccurate. 


5.2  Control  Approach 

The  control  approach  developed  here  consists  of  two  steps: 

•  First,  based  on  a  model  of  the  form  of  Equation  (5.1),  design  sequentially  LQG  sub¬ 
system  controllers  that  yield  desired  performance. 

•  Second,  make  the  subsystem  designs  robust  to  model  uncertainty  outside  the  frequency 
range  for  which  Equation  (5.1)  is  accurate. 


5.2.1  Sequential  Design  Process 


The  initial  subsystem  designs  are  performed  sequentially  based  on  a  model  of  the  form 


Xl{t) 

X2(i) 


All  Ai2 

0  A22 


Xi(f) 

X2(0 


Bii  Bi2 

0  B22 


ui(i) 

U2{t) 


(5.3) 


Exploiting  the  fact  that  the  coupling  between  the  subsystems  is  one  way  and  that  the 
control  ui(t)  does  not  affect  X2(^),  an  LQG  controller  is  designed  for  X2(t)  based  solely  on 
the  equation 


X2(i)  =  A22X2(f)  +B22U2(f).  (5.4) 

Specifically,  assuming  for  simplicity  that  full  knowledge  of  xi(t)  and  X2(t)  is  available,  a 
control  law  for  U2(t)  is  designed  of  the  form 

Mt)  = 


-K2X2(f). 


(5.5) 
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Substituting  Equation  (5.5)  into  Equation  (5.3)  results  in  the  following  system  equations 


Xi(t) 

.  *2^  . 

All 

0 


Ai2  —  B12K2 

A22  -  B22K2 


Xi(t) 

X2(t) 


+ 


Bn 

0 


Ul(t). 


(5.6) 


Equation  (5.6)  represents  a  system  containing  two  subsystems  that  are  one-way  coupled 
with  a  control  input  ui(t). 

Next,  based  on  Equation  (5.6),  a  control  law  for  ui(t)  is  designed  of  the  form 


ui{t)  =  -Kix(t) 


(5.7) 


where 


and 


x{t) 


Xi(t) 

X2(t) 


Ki 


Kii 


(5.8) 


(5.9) 


Equations  (5.5)  and  (5.7)  together  represent  a  total-system  controller  for  the  system  of 
Equation  (5.3).  The  corresponding  closed  loop  system  is  given  by 


Xi(t) 

.  *2(^)  _ 

All  —  BiiKii 
0 


Ai2  —  B12K2  -  B11K12 
A22  —  B22K2 


XI  (f) 

X2(t) 


(5.10) 


5.2.2  Subsystem  Control-Design  Approach 

For  the  resulting  total-system  controller  to  meet  desired  system  performance  objectives, 
the  subsystem  controllers  must  meet  corresponding  subsystem  performance  objectives.  An 
additional  requirement  is  that  the  subsystem  designs  be  robust  to  model  uncertainty.  This 
section  illustrates  the  delegation  of  subsystem  requirements  based  on  total-system  speci¬ 
fications,  the  initial  subsystem  control  designs  to  meet  these  requirements,  and  then  the 
approach  to  develop  the  corresponding  robust  subsystem  controllers. 


Subsystem  Performance  Requirements 

One  method  of  specifying  control-system  performance  requirements  for  flexible  manipulators 
is  to  establish  minimum  damping  ratios  and  frequencies  for  the  system  modes.  These 
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specifications  can  then  be  expressed  in  terms  of  regions  of  the  s  plane  in  which  the  LQG 
design  must  place  the  closed-loop  roots. 

The  one-way  coupling  characteristic  of  Equation  (5.1)  enables  system  performance  ob¬ 
jectives,  expressed  in  terms  of  closed-loop-root  regions,  to  be  partitioned  into  subsystem 
performance  requirements.  The  closed-loop  roots  Ac  are  the  eigenvalues  of  the  closed-loop 
system  matrix 

All  —  BiiKii  Ai2  —  B12K2  —  B11K12 
0  A22  —  B22K2 

Furthermore,  because  of  the  matrix  of  zeros  in  the  lower  left  corner,  Ac  can  also  be  expressed 
as 


Ac  =  {Ai,A2}  (5.12) 

where 

Ai  =  ef(7(Aii-BiiKii)  (5.13) 

and 

A2  =  eig  (A22  -  B22K2) .  (5-14) 

Physically,  Xi  represents  the  closed-loop  roots  of  subsystem  i.  Consequently,  designing 
the  subsystem  controllers  so  that  the  subsystem  eigenvalues  are  within  the  desired  regions 
implies  that  closed-loop  system  requirements  will  be  met. 

Initial  Subsystem  Control  Design 

The  subsystem  control  design  is  based  on  the  equation 

Xi{t)  =  AjXi(t) -F  BiUi(t).  (5.15) 

Assuming  full  state  feedback,  the  LQG  subsystem  design  involves  formulating  a  linear- 
quadratic  cost  function 

Ji  =  F  /  N(^rQxiXt(t) -F  Ui(t)'^Q„iUj(t)]dt,  (5.16) 

2  Jo 

and  then  minimizing  this  cost  function  wdth  respect  to  the  control  input  u,.  Qj,;  and 
are  weighting  matrices  that  enable  a  trade  off  between  performance  and  actuator  effort. 
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The  resulting  controller  is  a  gain  vector  Kj  [46],  and  the  corresponding  control  law  is  given 

by 


Ui{t)  =  -KiXj(t).  (5-17) 

The  subsystem  control  design  involves  varying  the  weighting  matrices  and  Q^.  until  a 
gain  matrix  Kj  results  that  places  the  subsystem  eigenvalues  Aj  within  the  desired  region. 

It  is  desirable  to  design  a  controller  that  regulates  the  subsystem  outputs  in  a  coordi¬ 
nated  fashion.  Given  the  controlled  outputs  of  subsystem  i,  G  IR”°%  expressed  as 

Yd  =  Cc,;Xi,  (5.18) 

the  weighting  matrix  is  selected  as 

Qx,  =  (5.19) 

where  £  j^noi^uoi  jg  ^  diagonal  matrix  that  w^eights  directly  the  controlled  outputs. 

Robust  Subsystem  Control  Design 

The  LQG  subsystem  controllers  are  designed  to  meet  performance  criteria  (i.e.  subsystem 
root  locations)  within  the  frequency  range  for  which  the  model  is  accurate.  Because  the 
model  is  inaccurate  outside  of  this  range,  a  mechanism  is  needed  to  maintain  performance 
in  the  frequency  range  of  interest  and  provide  robustness  to  model  uncertainty  outside  of 
this  range. 

This  can  be  done  followung  the  techniques  of  Gupta  [47]  (also  described  in  [9]).  The 
standard  subsystem  LQG  cost  function  given  in  Equation  (5.16),  can  be  transformed  to  the 
frequency  domain  using  Parseval’s  Theorem: 

/oo 

[x*(jcu)Qxi(iu;)xi(jw)  -1-  u*(jcu)Qu.(ia;)uj(ja;)]dcu  (5.20) 

-OO 

where  *  implies  complex  conjugate.  By  varying  Qxiij'^)  and  Qui(ji^)  over  frequency,  the 
performance  of  the  controller  can  be  made  a  function  of  frequency.  Since  the  objective  is  to 
vary  performance  over  frequency,  it  will  be  sufficient  to  vary  the  actuator  weighting  matrix 
only  resulting  in 

/OO 

K*(iw)QxiXi(ju;)  -I-  u*(jcj)Qui(juj)ui(jcj)jduj. 

-OO 


(5.21) 
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In  order  to  achieve  robust  subsystem  designs  and  still  meet  performance  requirements, 
the  strategy  is  to  make  control  effort  very  expensive  (large  Qu-  {ju)  and  hence  little  actuator 
response)  at  frequencies  where  the  model  is  inaccurate  and  to  make  it  cheap  (small  Q„,(ja;) 
and  hence  large  actuator  response)  at  frequencies  where  the  model  is  well  characterized. 
The  actuator  weighting  matrix,  must  be  positive  definite  such  that 

Q„,(iu;)  =  P*(ia;)Pi(iu;).  (5.22) 

The  cost  function  can  then  be  converted  back  to  the  time  domain  by  substituting  Equa¬ 
tion  (5.22)  into  Equation  (5.21) 

/OO 

[x|(ja;)Q^.Xi(ia;)  +  u*{jc^)PUM'PiMn^{j^)]duJ  (5.23) 

-OO 

/OO 

[xi(iw)Qj.Xi(ja;)  +  u*{juj)ui{juj)]duj  (5.24) 

-OO 

where 

Uiijuj)  =  Pi{juj)ui{ju).  (5.25) 

The  weighting  matrices  are  no  longer  a  function  of  frequency,  so  minimizing  the  frequency- 
domain  cost  function  is  equivalent  to  minimizing  the  time-domain  cost  function 

rr^ 

Ji^  =  /  +  (5.26) 

Jo 

UP{jio)  is  invertible  (if  P(ja;)  is  not  invertible,  see  [47]  for  an  alternative  implementa¬ 
tion),  Ui  satisfies 

Ui{juj)  =  P~^{juj)ui{juj)  (5.27) 

where  the  control  generated  by  the  control  design  must  be  physically  filtered  to  generate 
the  control  Ui  that  is  actually  sent  to  the  plant.  The  frequency-domain  problem  of  Equa¬ 
tion  (5.21)  can  now  be  solved  as  a  time-domain  problem  of  Equation  (5.26)  by  augmenting 
the  plant  with  the  filter  P~^(ja;).  The  prefilter  P“^(jc<;)  is  defined  by  the  state-space  model 

x/i  =  A/,x/;+B/.Ui  (5.28) 

Ui  =  C;,x_^^+D;.Ui  (5.29) 

where  the  output  of  the  filter,  Uj,  is  the  input  to  the  physical  plant.  The  filter  state  is 
defined  by  x/^  G  The  augmented  plant  equations  that  are  used  in  the  design  are 

given  by 


A5.X5,  -f 


(5.30) 
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where 


A...  - 


B,.  = 


Aj  BiCf. 


0 

BiD/; 

B/. 


Oi 


(5.31) 

(5.32) 


A  frequency-shaped  subsystem  controller  that  maintains  the  performance  of  the  initial 
subsystem  controller  is  achieved  by  solving  the  standard  LQG  problem  for  the  augmented 
plant  with  equal  to  identity  and 


where 


Csi  Cc,  OrioiXUf. 


(5,33) 


(5.34) 


and  Wxi  chosen  to  be  the  same  as  in  the  initial  subsystem  design  (see  Equation  (5.19)). 

5.2.3  Generalization 

This  sequential  control  approach  can  be  extended  to  predominantly  one-way  coupled  sys¬ 
tems  with  any  number  of  subsystems.  Specifically,  if  a  system  comprised  of  n  subsystems 
is  described  by 
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(5.35) 


within  the  frequency  range  of  interest,  then  a  total-system  controller  can  be  designed  based 
on  n  sequential,  robust,  subsystem  designs. 

The  specific  procedure  is  as  follows: 
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1.  Design  a  robust  control  law  for  actuator  n  of  the  form 


Unit)  =  -K„x„(t)  (5.36) 

from  the  equation 

^n{i)  —  (5.37) 

such  that  the  subsystem  closed-loop  roots  An  are  within  the  desired  region. 

2.  Incorporate  Equation  (5.36)  into  Equation  (5.35),  and  design  a  robust  control  law  for 
actuator  n  —  1  based  on  the  equations  describing  coordinates  Xn  and  Xn-i. 

3.  Repeat  step  2  sequentially  until  the  final  control  design  for  actuator  1  based  on  the 
equations  describing  coordinates  xi,X2,X3,  •  •  •  ,Xn. 


5.3  Example:  Control  Design  for  the  Experimental  System 

This  section  presents  an  application  of  the  control  approach  described  in  the  last  section 
to  develop  a  controller  for  the  experimental  system.  Because  the  mini-manipulator  tip 
dynamics  are  assumed  to  be  decoupled  from  the  main  arm  (see  Chapter  3),  the  control  design 
is  partitioned  into  designing  independent  main-arm  and  mini-manipulator  controllers. 


5.3.1  Main-Arm  Design 


As  described  in  Chapter  3,  the  main  arm  is  represented  accurately  as  a  predominantly  one¬ 
way  coupled  system.  Consequently,  within  the  frequency  range  of  interest  for  control,  the 
main-arm  dynamics  are  given  by 


xi(i) 

All  Ai2 

xi(i) 

+ 

Bii  Bi2 

uiit) 

X2(f)  _ 

0  A22 

_  X2(f)  _ 

0  B22 

u^it) 

(5.38) 


where  ui{t)  and  U2{t)  are  the  shoulder  and  elbow-motor  control  inputs,  and  xi(t)  and  X2(^) 
represent  the  dynamics  of  the  first-link  and  second-link  subsystems.  Presented  here  is  an 
application  of  the  sequential  control  approach  to  develop  a  regulator  for  the  main  arm.^ 

^Because  access  to  all  the  states  of  this  system  is  of  course  not  possible,  estimation  is  required.  Appendix  E 
details  the  estimator  design  for  the  main  arm. 
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Requirements  The  dynamic  response  of  the  main  arm  is  dominated  by  two  rigid-body 
modes  and  the  first  two  flexible  modes  of  the  system.  Consequently,  it  is  desired  to  achieve 
close  to  .707  damping  on  the  corresponding  closed- loop  roots,  while  at  the  same  time  push¬ 
ing  the  rigid-body  poles  well  beyond  the  limitations  corresponding  to  joint-based  control 
(i.e.  the  frequency  corresponding  to  the  first  cantilever  zero).  Because  of  the  one-way 
coupling  characteristics,  these  specifications  correspond  directly  to  subsystem  performance 
requirements.  Specifically,  each  subsystem  controller  is  required  to  achieve  close  to  .707 
damping  on  the  first  subsystem  flexible  mode  and  the  rigid-body  mode,  while  at  the  same 
time  pushing  the  subsystem  rigid-body  poles  beyond  the  first  cantilever  zero  of  the  subsys¬ 
tem. 


Control  Design  for  Subsystem  2 

Control  design  for  subsystem  two  is  based  on  the  model 

X2(t)  =  A22X22(t)  +  B22U2(t).  (5.39) 


The  controlled  outputs  for  subsystem  two,  defined  in  Figure  5.2,  are  given  by 


yc2  = 


'^tip2 

^tip2 


Cc2X2 


(5.40) 


where  vjtip2  and  6tip2  represent  the  lateral  deflection  and  rotation  of  the  tip  of  the  second 
link.  ^ 


Initial  Design  The  weighting  matrices  for  the  subsystem  designs  were  varied  until  the 
requirements  on  the  closed-loop  roots  were  satisfied.  The  final  weighting  matrices  for  the 
subsystem-two  design  are  defined  by 

=  l(N-m)-2  (5.41) 

Qx2  =  (5.42) 


with 


125  m-2  0 

0  25  rad“2 


(5.43) 


^Because  the  system  modes  corresponding  to  the  second  link  closely  resemble  a  pinned-pinned  beam, 
controlling  the  lateral  tip  deflection  wtip2  alone  is  insufficient.  To  meet  the  damping  requirements  on  the 
first  mode  of  subsystem  two,  it  is  also  necessary  to  control  the  rotation  at  the  tip  of  link  6tip2^ 
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Figure  5.2:  Controlled  Outputs  of  Subsystem  2 

This  figure  illustrates  the  controlled  outputs  of  subsystem  two.  Wtip2  6tip2 
represent  the  lateral  deBection  and  rotation  of  the  tip  of  link  2.  Also  shown  is  the 
elbow-motor  input  U2. 


The  resulting  control  law  is  of  the  form 

U2{t)  =  -K2X2(i).  (5.44) 

The  corresponding  closed-loop-root  locations  for  this  design  are  shown  in  Figure  5.3.  This 
controller  meets  the  damping  and  frequency  requirements  on  the  first  flexible  mode  and  the 
rigid-body  mode  of  subsystem  two.  The  experimental  response  of  this  controller,  however, 
is  unstable  as  shown  in  Figure  5,4.  With  the  manipulator  initially  at  rest,  the  controlled 
real  system’s  first  unmodelled  mode,  at  approximately  20  Hz,  is  seen  to  be  unstable.  Con¬ 
sequently,  the  design  of  the  subsystem-two  controller  needs  to  be  made  robust  to  these 
unmodelled  dynamics. 
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Closed-Loop  Roots  Magnified  View 


Figure  5.3:  Initial  Design  for  Subsystem  2 

This  figure  shows  the  closed-loop-root  locations  for  the  initial  control  design  for 
subsystem  two.  The  x’s  represent  the  open-loop  roots,  and  the  A^s  represent  the 
closed-loop  roots.  The  diagonal  dashed  line  corresponds  to  .707  damping.  The 
figure  on  the  right  is  a  magnified  view  of  the  figure  on  the  left.  The  quarter  circle 
corresponds  to  the  first- cantilever-mode  natural  frequency. 
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Figure  5.4:  Experimental  Response  of  Initial  Design  for  Subsystem  2 

Shown  in  the  hgure  is  the  elbow  torque  generated  by  the  initial  controller  for  sub¬ 
system  two.  With  the  manipulator  initially  at  rest^  the  first  unmodelled  mode  of 
the  controlled  system,  at  approximately  20  Hz,  is  seen  to  be  unstable. 
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Robust  Design  To  make  the  initial  design  for  subsystem  two  robust  to  the  unmodelled 
dynamics,  a  frequency-shaped  design  was  achieved  by  implementing  a  fifth-order  Butter- 
worth  filter  with  a  5  Hz  cut-off  frequency.  The  corresponding  closed-loop  roots  for  this 
design  are  given  in  Figure  5,5.  The  figure  contains  additional  open-loop  and  closed-loop 


Closed-Loop  Roots 


Magnified  View 


Figure  5.5:  Robust  Design  for  Subsystem  2 

This  figure  shows  the  closed-loop  root  locations  for  the  robust  control  design  for 
subsystem  two.  This  figure  contains  additional  open-loop  poles  due  to  the  prefilters. 
The  hlter  closed-loop  roots  are  represented  by  the  black  triangles.  Because  of  the 
high  cost  of  control  at  high  frequencies,  the  controller  makes  essentially  no  effort 
to  damp  the  second  hexible  mode  of  subsystem  two.  However,  comparison  of  this 
hgure  with  Figure  5.3  indicates  that  the  root  locations  of  the  rigid-  body  mode  and 
the  first  mode  are  unaffected  by  the  frequency- weighted  design. 


poles  due  to  augmenting  the  filter  to  the  plant.  The  filter  closed-loop  roots  are  represented 
by  the  black  triangles.  The  frequency-shaped  design  limits  the  output  of  the  feedback  con¬ 
troller  above  5  Hz.  Because  of  the  high  cost  of  control  at  high  frequencies,  the  controller 
makes  essentially  no  effort  to  damp  the  second  flexible  mode  of  subsystem  two.  However, 
comparison  of  this  figure  with  Figure  5.3  indicates  that  the  root  locations  of  the  rigid  body 
and  first  mode  are  affected  very  little  by  the  frequency-weighted  design. 

The  robustness  of  this  design  is  verified  by  the  experimental  response  of  Figure  5.6. 
In  this  response,  the  initial  subsystem  controller  is  active  from  1.5  to  3  seconds.  At  three 
seconds,  a  control-swap  is  performed,  and  the  robust  subsystem  controller  is  activated.  The 
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Figure  5.6;  Experimental  Response  of  Robust  Design  for  Subsystem  2 

In  this  response,  the  initial  subsystem  controller  is  active  from  1.5  to  3  seconds. 
At  3  seconds,  a  control-swap  is  performed,  and  the  robust  subsystem  controller  is 
activated.  The  frequency-shaped  controller  stabilizes  the  system,  and  this  torque 
history  demonstrates  the  robustness  of  this  design  to  high-frequency  modelling  un¬ 
certainty. 


frequency-shaped  controller  stabilizes  the  system,  and  this  torque  history  demonstrates  the 
robustness  of  this  design  to  high-frequency  modelling  uncertainty. 


Control  Design  for  Subsystem  1 


With  the  second-subsystem  controller  designed,  a  controller  for  subsystem  one  was  designed 
in  a  similar  fashion  based  on  a  model  of  the  form 


Xi(t) 

All  Ai2 

-  B12K2 

Xi(t) 

4- 

’  Bii  ■ 

X2(t) 

O 

> 

to 

to 

—  B22K2 

X2(t) 

0 

The  controlled  outputs  for  subsystem  one,  defined  in  Figure  5.7,  are  given  by 


(5.45) 


Yci  = 


^tipl 


CciXi 


(5.46) 


where  wupi  and  9tipi  represent  the  lateral  deflection  and  rotation  of  the  tip  of  link  1. 
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Figure  5.7:  Controlled  Outputs  of  Subsystem  1 

This  figure  illustrates  the  controlled  outputs  of  subsystem  one.  wupi  and  6tipi 
represent  the  lateral  deflection  and  rotation  of  the  tip  of  link  1.  Also  shown  is  the 
shoulder-motor  input  ui. 


Initial  Design  The  weighting  matrices  for  the  first-subsystem  design  were  varied  until 
the  requirements  on  the  closed-loop  roots  were  satisfied.  The  final  weighting  matrices  for 
the  subsystem-one  design  are  given  as 


with 


=  l(N-m)-2 


100  m-2  0 

0  20  rad-2 


(5.47) 

(5.48) 


(5.49) 
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The  resulting  control  law  is 


Ui{t)  =  -Kix(t) 


(5.50) 


where 


and 


(5.51) 


Ki  -  [  Kn  Ki2  ]  .  (5.52) 

The  corresponding  subsystem-one  closed-loop-root  locations  for  this  design  are  shown  in 
Figure  5.8.  Both  the  rigid-body  mode  and  the  first  flexible  mode  meet  the  damping  speci¬ 
fications,  and  the  rigid-body  poles  are  well  beyond  the  frequency  of  the  first-cantilever  zero 
of  subsystem  one. 


Closed-Loop  Roots  Magnified  View 
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Figure  5.8:  Initial  Design  for  Subsystem  1 

This  figure  shows  the  subsystem-one  closed-loop  root  locations  for  the  initial  control 
design.  The  x ’s  represent  the  open-loop  roots,  and  the  A ’s  represent  the  closed-loop 
roots.  The  diagonal  dashed  line  corresponds  to  .707  damping.  The  figure  on  the 
right  is  a  magnified  view  of  the  figure  on  the  left.  The  quarter  circle  corresponds 
to  the  hrst-cantilever-mode  natural  frequency. 
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Robust  Design  From  the  initial  design  for  subsystem  one,  the  frequency  shaped-design 
was  obtained  by  augmenting  the  plant  with  a  fifth-order  Butterworth  filter  with  a  5  Hz  cut¬ 
off  frequency.  The  resulting  closed-loop  roots  for  subsystem  one  are  shown  in  Figure  5.9. 
Because  of  the  high  cost  of  control  at  high  frequencies,  the  controller  makes  essentially  no 


Closed-Loop  Roots 
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Magnified  View 


Figure  5.9:  Robust  Design  for  Subsystem  1 

This  hgure  shows  the  subsystem-one  closed-loop  root  locations  for  the  robust  control 
design.  This  figure  contains  additional  open-loop  poles  due  to  the  prehlters.  The 
filter  closed-loop  roots  are  represented  by  the  black  triangles.  Because  of  the  high 
cost  of  control  at  high  frequencies,  the  controller  makes  essentially  no  effort  to  damp 
the  second  hexible  mode  of  subsystem  one.  However,  comparison  of  this  figure  with 
Figure  5.8  indicates  that  the  root  locations  of  the  rigid-  body  mode  and  the  first 
mode  are  essentially  unaffected  by  the  frequency-weighted  design. 


effort  to  damp  the  second  flexible  mode  of  subsystem  one.  There  is  a  slight  difference  in  the 
first-flexible-mode  root  locations  between  the  robust  design  and  the  initial  design.  This  is 
due  to  the  prefilter  cut-off  frequency  being  very  close  to  the  frequency  of  the  damped  flexible 
mode.  However,  this  discrepancy  was  not  sufficiently  significant  to  warrant  redesign. 


Resulting  Closed-Loop  System 

Equations  (5.44)  and  (5.50)  together  represent  a  total-system  controller  for  the  system  of 
Equation  (5.38).  The  gain  matrices  are  given  in  Appendix  F.  The  corresponding  closed-loop 
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system  is  given  by 


Xl(t) 

All  “  BiiKii  Ai2  —  B12K2  —  B11K12 

Xi(t) 

X2(0 

0  A22  ~  B22K2 

X2(t) 

(5.53) 


The  corresponding  closed-loop  roots  of  the  plant  are  given  in  Figure  5.10.  Figure  5.11 


Closed-Loop  Roots 


Magnified  View 
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Figure  5.10:  System  Controller 

This  figure  shows  the  closed-loop- root  locations  corresponding  to  the  total-system 
controller. 


shows  the  experimental  response  of  the  closed-loop  system  in  following  a  desired  end-point 
trajectory.  From  the  desired  end-point  position,  corresponding  joint  angles  are  determined 
through  inverse  kinematics  assuming  rigid  links.  The  manipulator  is  then  commanded 
to  follow  the  desired  motor  positions.  The  corresponding  end-point  response  is  shown  in 
Figure  5.11,  along  with  the  motor  torque  histories.  Because  the  controller  is  error  based, 
the  actual  end-point  position  trails  the  desired  position.  This  response  illustrates  the  need 
for  feedforward  control  to  follow  more  closely  a  desired  state  history. 
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End-Point  Motion  End-Point  Motion 


Figure  5.11:  Experimental  Response 

This  figure  shows  the  response  of  the  closed-loop  system  in  following  a  desired  end¬ 
point  trajectory.  Because  the  controller  is  error  based,  the  actual  end-point  position 
trails  the  desired  position.  This  response  illustrates  the  need  for  feedforward  control 
to  follow  more  closely  a  desired  state  history 
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5.3.2  Mini-Manipulator  Control  Design 


The  goal  of  the  mini-manipulator  control  design  is  to  generate  the  torques  t\  and  T2  to 
control  the  mini-manipulator  tip  to  the  desired  tip  position  in  inertial  space.  As  described 
in  Chapter  3,  the  dynamics  of  the  mini-manipulator  can  be  well  represented  as  a  tip  mass 
controlled  by  linear  actuators.  The  corresponding  equations  of  motion  are  given  by 


Xfflrp 

■  0 
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(5.54) 


where  Xm-r  a^nd  ymr  the  tip  position  relative  to  the  base  as  shown  in  Figure  5.12.  For 


Figure  5.12:  Mini-Manipulator  Subsystem 

The  goal  of  the  mmi-manipulator  control  design  is  to  generate  the  torques  ti  and 
T2  to  control  the  mini-manipulator  tip  to  the  desired  tip  position  in  inertial  space. 


the  system  of  Equation  (5.54),  a  linear-quadratic-regulator  (LQR)  design  was  performed. 
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The  weighting  matrices  chosen  were 


Q«  — 


1.2  N-2  0 

0  1.2  N-2 

10000  m-2  0  0  0 

0  10000  m-2  0  0 


(5.55) 


(5.56) 


The  regulator  is  of  the  form 


where 


F  =  -Kr,x  -  K„x 


(5.57) 


(5.58) 


(5.59) 


(5.60) 


and  Kp  and  are  the  2  x  2  resulting  gain  matrices.  The  corresponding  closed-loop-root 
locations  are  shown  in  Figure  5.13.  Because  the  dynamics  of  the  mini  in  both  the  Xmr  ^'^d 
Xmr  coordinates  and  the  weighting  on  these  coordinates  were  identical,  the  LQR  design 
placed  the  closed-loop  roots  at  the  same  location. 

Implementation 

The  controller  in  Equation  (5.57)  was  implemented  as  a  joint-based  controller.  For  small 
displacements,  the  corresponding  joint  controller  is  given  by 


where 


(5.61) 


(5.62) 
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Closed-Loop  Roots 


Figure  5.13:  Mini-Manipulator-Subsystem  Closed-Loop  Roots 

The  figure  shows  the  closed-loop-root  locations  corresponding  to  the  mini¬ 
manipulator  control  design.  Because  the  dynamics  of  the  mini  in  both  the  Xmr 
and  XjriT  coordinates  and  the  weighting  on  these  coordinates  were  identical^  the 
LQR  design  placed  the  closed-loop  roots  at  the  same  location. 


e  - 

e  - 

and  J  is  the  mini-manipulator  Jacobian  (given 
ulator  configuration. 

Experimental  Results 

Figures  5.14  and  5.15  show  step  responses  of  the  mini-manipulator  motors.  The 

corresponding  tip  motion  is  primarily  in  the  X  direction  in  Figure  5.14,  and  primarily  in 
the  Y  direction  in  Figure  5.15. 

Using  the  approximate  relationships  given  in  [48]  between  rise  time  and  bandwidth 
tr  ^  l>8/u)n  ,  and  overshoot  and  damping,  the  responses  in  Figures  5.14  and  5.15  reflect  a 


01 

02 

01 

02 


(5.63) 


(5.64) 


in  Appendix  B)  for  a  nominal  mini-manip- 
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Figure  5.14;  Experimental  Step  Response 

The  motor  responses  in  this  figure  correspond  to  tip  motion  in  primarily  the  X 
direction. 


bandwidth  of  approximately  25  rad/ sec.  and  an  approximate  damping  ratio  of  .7.  These 
values  correspond  closely  to  the  theoretical  closed-loop  roots  shown  in  Figure  5.13. 

Given  the  performance  of  the  joint-based  controller,  controlling  the  mini- manipulator 
tip  to  a  desired  target  requires  specifying  appropriate  desired  joint  angles  and  angular  rates. 
When  the  target  is  within  reach,  these  desired  values  are  calculated  based  on  vision-sensor 
information  and  inverse-kinematic  relations  given  in  Appendix  B.  When  the  target  is  out 
of  reach,  the  desireds  correspond  to  the  point  on  the  boundary  of  the  mini-manipulator 
workspace  that  is  closest  to  the  desired  target.  Figure  5.16  demonstrates  the  ability  to 
generate  suitable  joint  commands  to  regulate  the  mini-manipulator  tip  to  a  desired  target 
in  the  presence  of  base  motions.  With  the  main-arm  controller  off,  the  base  of  the  mini¬ 
manipulator  is  displaced  as  shown.  The  induced  error  in  the  mini-manipulator-tip  position 
is  also  shown.  By  specifying  the  proper  joint  commands,  and  then  regulating  the  joints 
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Left  Motor  Right  Motor 


Figure  5.15;  Experimental  Step  Response 

The  motor  responses  in  this  figure  corresponds  to  tip  motion  in  primarily  the  Y 
direction.  Using  the  approximate  relationships  given  in  [48]  between  rise  time  and 
bandwidth  (U  «  1.8/a;„J  ,  and  between  overshoot  and  damping,  the  responses  in 
this  figure  and  Figure  5.14  reBect  a  bandwidth  of  approximately  25  rad/ sec.  and 
an  approximate  damping  ratio  of  .7. 


to  these  commands,  the  mini-manipulator  is  able  to  compensate  for  the  base  motion  and 
regulate  effectively  the  tip  to  the  desired  target. 

The  redundancy  introduced  by  the  mini-manipulator  also  improves  tip-disturbance  re¬ 
jection  of  the  system  as  shown  in  Figure  5.17.  Figure  5.17  presents  two  cases  in  which 
an  approximately  constant  tip  disturbance  is  applied.  In  the  plot  on  the  left,  the  joints  of 
the  mini-manipulator  are  locked,  and  a  tip  disturbance  is  applied  to  the  mini-manipulator 
tip.  The  disturbance  rejection  in  this  case  is  provided  entirely  by  the  shoulder  and  elbow 
torques  generated  by  the  main-arm  controller.  The  corresponding  induced  tip  errors  are 
much  larger  than  in  the  plot  on  the  right  where  the  mini-manipulator  controller  is  active. 
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Figure  5.16:  Mini-Controller  Base-Disturbance  Rejection 

This  figure  shows  experimental  results  in  which  the  main-arm  controller  is  turned 
off  and  the  base  of  the  mini-manipulator  is  displaced  as  shown.  The  induced  er¬ 
ror  in  the  mini-manipulator-tip  position  is  also  shown.  By  specifying  the  proper 
joint  commands  based  on  end-point  sensing  of  the  mini-manipulator^s  tip  location, 
and  then  regulating  the  Joints  to  these  commands,  the  mini-manipulator  is  able  to 
compensate  for  the  base  motion  and  regulate  the  tip  to  the  desired  target. 


The  plot  on  the  right  illustrates  the  ability  of  the  redundant  system  to  keep  tip  errors  very 
small  in  the  presence  of  a  tip  disturbance. 
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Figure  5.17:  Tip-Disturbance  Rejection 

This  figure  presents  two  cases  in  which  an  approximately  constant  tip  disturbance 
is  applied.  In  the  plot  on  the  left,  the  joints  of  the  mini-manipulator  are  locked,  and 
a  tip  disturbance  is  applied  to  the  mini-manipulator  tip.  The  disturbance  rejection 
in  this  case  is  provided  entirely  by  the  shoulder  and  elbow  torques  generated  by  the 
main-arm  controller.  The  corresponding  induced  tip  errors  are  much  larger  (a  factor 
of  almost  25)  than  in  the  plot  on  the  right  where  the  mini-manipulator  controller 
is  active.  The  plot  on  the  right  illustrates  the  ability  of  the  redundant  system  to 
keep  tip  errors  very  small  in  the  presence  of  a  tip  disturbance. 
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5.4  Summary 

This  chapter  has  presented  the  feedback-control  approach  developed  in  this  research  for 
predominantly  one-way  coupled  systems.  This  approach  exploits  the  one-way  coupling  that 
characterizes  these  systems  within  the  frequency  range  of  interest  for  control.  As  a  result, 
the  development  of  a  total-system  controller  is  partitioned  into  sequential  subsystem  designs. 
Because  the  one-way  coupling  characteristic  is  inaccurate  outside  of  the  frequency  range  of 
interest,  the  subsystem  controllers  are  made  to  be  robust  to  this  model  uncertainty.  Robust 
subsystem  controllers  are  achieved  through  a  frequency-shaped  control  design. 

This  approach  has  been  applied  to  the  experimental  apparatus.  With  this  approach, 
control  design  for  the  main  arm  was  partitioned  into  two  sequential  subsystem  control 
designs.  These  subsystem  controllers  were  designed  to  meet  performance  specifications  (in 
the  form  of  desired  closed-loop  regions)  within  the  frequency  range  of  interest,  while  at 
the  same  time  achieving  robustness  to  model  uncertainty  outside  of  this  frequency  range. 
The  development  of  a  total-system  controller  was  completed  with  the  mini-manipulator 
control  design.  The  resulting  controller  will  be  used  in  the  following  chapter  to  augment 
the  control  input  of  the  trajectory  generator  in  a  point-to-point  repositioning  of  the  mini¬ 
manipulator/main-arm  experimental  system. 


Chapter  6 


“Putting  It  All  Together” 


Chapters  4  and  5  described  the  approaches  developed  in  this  dissertation  to  design  the 
trajectory-generator  and  feedback-controller  components  of  the  control-system  framework 
described  in  Chapter  1.  This  chapter  presents  experimental  results  demonstrating  the 
performance  of  this  control  strategy  on  the  mini-manipulator/main-arm  system. 


Feedforward 

Control 


Figure  6.1:  Control  Framework 

Chapters  4  and  5  described  the  approaches  developed  in  this  dissertation  to  design 
the  trajectory-generator  and  feedback- controller  components  of  the  solution  frame¬ 
work  described  in  Chapter  1.  This  chapter  present  experimental  results  demon¬ 
strating  the  performance  of  this  control  strategy  on  the  mini-manipulator/main-arm 
system. 
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“PUTTING  IT  ALL  TOGETHER” 

6.1  Interface  to  the  Experimental  System 

A  graphical  user  interface,  shown  in  Figure  6.2,  has  been  developed  to  enable  the  user  to 
specify  commands  to  the  system  in  an  intuitive  fashion.  The  user  interface  displays  icons 
of  the  mini-manipulator  tip  and  desired  targets.  These  icons  represent  the  actual  positions 
provided  by  the  vision  sensor.  The  interface  enables  the  user  to  specify  final  tip  positions 
by  using  a  computer  mouse  to  drag  the  icon  of  the  mini-manipulator  to  a  position  on  the 
screen,  or  by  clicking  on  one  of  the  target  icons.  Clicking  on  one  of  the  targets  sets  the 
desired  tip  position  to  the  location  of  the  center  of  the  target.  The  slew  time  is  specified 
by  clicking  on  the  desired  time. 


6.2  Experimental  Point-to-Point  Repositionings 

To  illustrate  the  contributions  of  both  the  feedforward  and  feedback  inputs  to  the  system, 
results  corresponding  to  two  cases  are  presented  (see  Figure  6.3).  In  the  first  case,  the 
feedforward-controhinput  connection  is  broken.  The  resulting  response  is  due  to  the  feed¬ 
back  controller  attempting  to  regulate  the  plant  to  the  desired  state  history  provided  by 
the  trajectory  generator.  In  the  second  case,  both  the  feedforward  and  feedback  inputs  are 
sent  to  the  plant,  representing  the  performance  of  the  control  strategy  developed  in  this 
thesis.  In  addition,  for  both  cases,  results  are  presented  with  the  mini-manipulator  active 
and  inactive  to  illustrate  the  advantages  of  the  mini-manipulator. 

For  these  experiments,  the  “acceptable  time  for  repositioning”  is  set  arbitrarily  at  2.5 
seconds.  The  experimental  response  for  each  case  is  presented  in  four  plots.  The  tip 
error  represents  the  distance  between  the  mini-manipulator-tip  position  and  the  desired 
mini-manipulator-tip  position.  The  base  error  is  the  distance  between  the  position  of  the 
mini-manipulator  base  and  the  nominal  steady-state  location  of  the  base  that  corresponds 
to  the  desired  final  configuration.  Also  shown  is  the  feedforward-control  input  from  the 
trajectory  generator  and  the  total  control  (feedforward  plus  feedback)  sent  to  the  plant  for 
both  the  shoulder  and  elbow  actuators.  Figures  6.4  through  6.7  correspond  to  2.5  second 
repositionings  in  which  the  mini-manipulator  tip  is  commanded  to  move  from  (.9279,  .4904) 
to  (.6394,  .7675)  where  the  X  and  Y  coordinates  are  defined  in  Figure  4.6. 


6.2.  EXPERIMENTAL  POINT-TO-POINT  REPOSITIONINGS 
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Figure  6.2:  Graphical  User  Interface  to  the  Experimental  System 

This  graphical  user  interface  has  been  developed  to  enable  the  user  to  specify  com¬ 
mands  to  the  system  in  an  intuitive  fashion. 
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Figure  6.3:  Experimental  Cases 

To  illustrate  the  contributions  of  both  the  feedforward  and  feedback  inputs  to  the 
system,  results  corresponding  to  two  cases  are  presented.  In  Case  I,  the  feedforward- 
control-input  connection  is  broken.  The  resulting  response  is  due  to  the  feedback 
controller  attempting  to  regulate  the  plant  to  the  desired  state  history.  In  Case  II, 
both  the  feedforward  and  feedback  inputs  are  sent  to  the  plant,  representing  the 
performance  of  the  control  strategy  developed  in  this  thesis. 


6,2.1  Case  I:  Feedback  Only 

Mini-Manipulator  Inactive  In  the  response  of  Figure  6.4,  the  main  arm  is  controlled 
by  feedback  alone,  and  the  mini-manipulator  joints  are  locked.  The  performance  represents 
the  ability  of  feedback  control  with  the  main  arm  only  to  track  the  optimal  desired  state 
history  produced  by  the  trajectory  generator.  With  feedback  and  the  main  arm  alone,  the 
system  is  not  able  to  meet  the  repositioning-time  requirement  of  2.5  seconds;  it  completes 
the  repositioning  in  approximately  5.2  seconds. 


Mini-Manipulator  Active  In  the  response  of  Figure  6.5,  the  main  arm  is  controlled  by 
feedback  alone,  now  with  the  mini-manipulator  active.  Without  feedforward,  the  system 
is  still  not  able  to  quite  meet  the  2.5  second  repositioning-time  requirement,  but  the  mini¬ 
manipulator  is  able  to  acquire  the  target  after  3.2  seconds:  the  mini-manipulator  achieves 
a  2  second  decrease  in  the  repositioning  time  compared  to  the  previous  response. 
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Figure  6.4;  Experimental  Response:  Feedback  Only/Mini-Manipulator  Inactive 
The  tip  reaches  the  target  in  5.2  seconds. 


Figure  6.5:  Experimental  Response:  Feedback  Only/Mini-Manipulator  Active 

The  tip  reaches  the  target  in  3.2  seconds. 
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6.2.2  Case  II:  Feedback  and  FeedForward 

Mini-Manipulator  Inactive  In  the  response  of  Figure  6.6,  the  main  arm  is  controlled 
by  both  feedback  and  feedforward  control,  and  the  mini-manipulator  joints  are  locked. 
The  repositioning  is  completed  in  approximately  4.5  seconds.  Comparison  with  Figures  6.4 


Figure  6.6:  Feedback  and  FeedForward/Mini-Manipulator  Inactive 

The  tip  reaches  the  target  in  4.5  seconds. 


and  6.5  indicate  that  the  use  of  feedforward  and  feedback  does  result  however  in  a  more 
efficient  use  of  control  effort,  since  the  total  control  effort  and  peak  control  effort  used  in 
Figure  6.6  is  less  than  in  Figures  6.4  and  6.5.  However,  without  the  mini-manipulator  the 
system  is  still  unable  to  meet  the  repositioning-time  requirement. 

Mini-Manipulator  Active  In  the  response  of  Figure  6.7,  the  main  arm  is  controlled  by 
both  feedback  and  feedforward  control,  and  the  mini-manipulator  is  active.  This  response 
represents  the  culmination  of  this  research.  The  mini-manipulator  is  now  able  to  acquire 
the  target  within  2.5  seconds.  Consequently,  the  control  strategy  merging  feedback  and 
feedforward  control  is  able  to  meet  the  stringent  repositioning  requirement  selected  for  the 
experimental  system. 

Figure  6.8  shows  a  response  for  the  same  initial  and  final  tip  positions  with  a  3  second 
selection  for  final  time,  instead  of  the  2.5  second  selection  used  up  to  here.  An  interesting 
observation  of  this  response  is  that  less  feedback-control  input  is  required  than  in  the  2.5 
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Figure  6.7:  Feedback  and  FeedForward/Mini-Manipulator  Active 

The  tip  reaches  the  target  in  2.5  seconds. 


Figure  6.8:  Feedback  and  FeedForward/Mini-Manipulator  Active:  tf  ~  3,0  Sec¬ 
onds 


The  tip  still  reaches  the  target  in  2.5  seconds. 
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second  slew.  This  is  because  in  the  3  second  slew,  there  is  significantly  less  link  deflection. 
Consequently,  the  model  of  the  main  arm  that  was  developed  assuming  small  link  deflections 
is  more  accurate  during  this  repositioning.  Thus,  the  plant  state  follows  the  model  state 
closely  from  the  feedforward  input,  and  less  feedback  is  drawn  upon. 

Finally,  for  comparison.  Figure  6.9  shows  a  3  second  repositioning  corresponding  to  a 
relatively  small-motion  repositioning.  The  mini-manipulator  tip  is  commanded  to  move 
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Figure  6.9:  Small-Motion  Repositioning:  tf  =  3.0  Seconds 

The  tip  still  reaches  the  target  in  2.5  seconds. 


from  (.5069,  .6792)  to  (.6390,  .7676),  a  total  distance  of  15  cm  compared  with  38  cm  in 
the  previous  tests.  As  in  the  response  of  Figure  6.8,  the  corresponding  link  deflections  are 
small  and  the  manipulator  is  controlled  primarily  from  the  feedforward-control  signal.  The 
feedback  loop  is  called  upon  for  only  minor  additions  to  the  feedforward-control  input. 

Furthermore,  Figure  6.9  also  supports  the  the  primary  tenet  of  this  thesis.  Specifically, 
once  the  base  of  the  mini-manipulator  is  within  an  area  (indicated  approximately  by  the 
shaded  region  in  Figure  6.9),  the  mini-manipulator  is  able  to  acquire  the  desired  position 
accurately  and  quickly. 
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6.3  Summary 

This  chapter  presented  experimental  results  that  verify  the  ability  of  the  control  strategy 
developed  in  this  thesis  to  reposition  quickly  the  mini-manipulator/main-arm  system.  From 
these  experiments,  a  number  of  observations  were  made.  First,  without  both  feedforward 
and  feedback  control,  the  stringent  (2.5  second)  time  requirement  for  the  repositioning 
command  is  not  met.  Second,  for  the  2.5  second  repositionings  presented  here,  meeting  the 
repositioning-time  requirement  was  only  possible  with  the  mini-manipulator  active  (without 
it,  the  move  took  at  best  4.3  seconds).  Third,  the  combination  of  feedback  and  feedforward 
control  resulted  in  a  more  efficient  use  of  control  effort  than  using  feedback  alone  to  follow 
the  desired  state  erformance  was  achieved  with  less  peak  and  total  control  usage. 
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Chapter  7 

Conclusions 


This  final  chapter  consists  of  two  sections.  The  first  section  summarizes  the  findings  of  this 
research.  The  second  gives  suggestions  for  future  research. 

7.1  Summary 

This  research  has  addressed  increasing  the  end-point  performance  of  multi-link  flexible 
manipulators  through  the  use  of  a  mini-manipulator^  or  small,  high-bandwidth  rigid  robot. 
Mounting  a  mini-manipulator  at  the  tip  of  a  multi-link  flexible  manipulator  introduces 
high-bandwidth,  local-manipulation  capability  and  redundant  degrees  of  freedom  that,  in 
conjunction  with  direct  end-point  sensing,  enable  high-performance  end-point  control  in  the 
presence  of  link  flexibility  that  is  substantial. 

Contributions  made  in  the  course  of  this  research  are  summarized  in  Section  1.5.  The 
work  has  led  to  the  conclusions  that  follow, 

7.1.1  Exploiting  the  Speed  and  Redundancy  of  the  Mini-Manipulator 

To  exploit  the  high-bandwidth  local-manipulation  capability  and  the  redundancy  introduced 
by  the  mini-manipulator,  a  new  control  strategy  has  been  developed  for  multi-link  flexible 
manipulators.  Because  the  mini-manipulator  provides  quick,  precise  control,  but  only  within 
a  localized  workspace,  the  end-point  performance  of  the  overall  system  is  limited  by  the 
speed  at  which  the  flexible  main  arm  can  transport  the  mini-manipulator.  As  a  result, 
this  new  strategy  has  focused  on  controlling  quickly  the  flexible  main  arm  to  a  target  area^ 
rather  than  to  a  precise  location,  from  which  the  mini-manipulator  can  obtain  the  desired 
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tip  position.  This  new  concept  of  controlling  to  a  target  area  was  pursued  in  this  research 
through  a  new  technique  known  as  soft  terminal  control  The  experiments  performed  have 
demonstrated  the  ability  to  control  to  an  area  much  more  quickly  than  to  a  specific  point. 

7.1,2  Soft  Terminal  Control  of  Multi-Link  Flexible  Manipulators 

Soft  terminal  control  of  a  multi-link  flexible  manipulator  is  made  possible  by  the  speed  and 
redundancy  introduced  by  the  mini-manipulator.  Because  of  this  redundancy,  the  target 
can  be  acquired  with  the  system  in  more  than  one  configuration.  Rather  than  controlling 
the  flexible  main  arm  to  a  specific  point,  high  performance  of  the  overall  system  can  be 
achieved  by  quickly  bringing  the  main  arm  near  the  target,  to  a  target  area;  an  area  within 
which  the  speed  and  precision  of  the  mini-manipulator  can  then  be  utilized  to  perform  the 
task  at  hand. 

A  new  soft-terminal-control  approach  has  been  developed  for  multi-link  flexible  manip¬ 
ulators.  This  approach  combines  feedforward  control,  produced  by  a  trajectory  generator, 
with  feedback  control  to  control  quickly  the  main  arm  to  the  target  area.  The  culmination 
of  this  research  was  an  experimental  verification  of  this  approach  on  an  extremely  flexible 
two-link  manipulator  with  a  mini-manipulator  mounted  at  its  tip.  Experimental  results 
demonstrated  increased  performance  offered  by  the  mini-manipulator  using  this  combina¬ 
tion  of  feedforward  and  feedback  control.  For  the  system  repositionings  presented  in  this 
dissertation,  the  mini-manipulator  enabled  nearly  a  factor  of  two  decrease  in  repositioning 
times.  Also,  the  mini-manipulator  reduced  tip  errors  due  to  tip  disturbances  by  a  factor  of 
25. 

In  developing  this  new  soft-terminal-control  approach,  fundamental  advances  were  made 
in  trajectory  generation,  feedback-control  design,  and  modelling  of  multi-link  flexible  ma¬ 
nipulators. 

Trajectory  Generation 

A  new  trajectory-generation  approach  for  multi-link  flexible  manipulators  has  been  devel¬ 
oped  that  incorporates  into  the  trajectory  generation  the  concept  of  controlling  to  a  target 
area  rather  than  to  a  specific  point.  This  approach  generates  both  the  optimal-state  his¬ 
tory  and  the  corresponding  feedforward-control  input  for  repositioning  multi-link  flexible 
manipulators.  This  approach  is  based  on  an  explicit-model-following  implementation  of  a 
terminal  controller,  in  which  the  terminal  controller  drives  a  plant  model  to  the  desired 
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plant  state.  The  output  of  the  terminal  controller  is  the  optimal  control  for  the  reposition¬ 
ing,  and  is  fed  forward  to  the  plant.  The  corresponding  plant-model  state  history  is  the 
optimal  state  history  for  the  repositioning,  and  represents  the  desired  motion  of  the  plant. 
The  target-area  concept  is  incorporated  into  the  terminal-controller  design  in  the  form  of 
soft  terminal  constraints. 

A  method  has  been  developed  to  shape  the  frequency  content  of  the  generated  trajec¬ 
tories  so  as  not  to  excite  lightly-damped  unmodelled  flexible  modes.  This  method  involves 
frequency  weighting  the  finite-time,  linear-quadratic  cost  function  in  the  terminal-controller 
design.  It  worked  very  well.  As  a  result,  this  approach  is  directly  applicable  to  manipula¬ 
tors  exhibiting  significant  link  flexibility.  Furthermore,  time  weighting  of  the  cost  function 
and  implementation  of  the  terminal  controller  in  a  model-following  control  framework  did 
in  fact  enable  an  essentially  bumpless  control-mode  transfer  at  the  end  of  the  large-motion 
slews. 


Feedback-Control  Design 

As  a  first  step  toward  reducing  the  high-performance-control  design  problem  for  general 
multi-link  flexible  manipulators  into  smaller  more  tractable  subsets,  a  sequential  control- 
design  approach  for  predominantly  one-way-coupled  systems  has  been  developed.  By  ex¬ 
ploiting  this  limited-coupling  characteristic,  this  control  approach  enabled  a  straightforward 
sequential  design  process.  The  subsystem  controllers  were  made  robust  to  model  uncertainty 
outside  the  frequency  range  of  interest  through  a  frequency-shaped  control  design.  Because 
system-level  coupling  information  was  fully  utilized  in  this  process,  and  not  overlooked,  high 
performance  was  achieved;  yet,  through  a  sequential  control-design  process. 


Modelling 

A  modular  approach  to  modelling  multi-link  flexible  manipulators  has  been  developed.  In 
this  approach,  a  total-system  model  is  generated  through  a  concatenation,  or  integration, 
of  state-space  subsystem  models.  This  technique  enables  the  concatenation  by  enforcing 
the  physical  constraints  that  couple  the  subsystems.  Thus,  a  total-system  model  can  be 
developed  by  first  developing  accurate  subsystem  models,  and  then  integrating  to  generate 
a  total-system  mathematical  representation. 
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This  modelling  approach  was  applied  to  develop  an  accurate  model  of  the  experimental 
two-link  flexible  manipulator  with  a  mini-manipulator  at  its  tip.  This  model  was  used  in 
the  design  of  optimal  trajectories  and  of  a  feedback  controller  for  the  experimental  system. 

7.2  Recommendations  for  Future  Work 

Performing  this  research  generated  a  number  of  ideas  for  possible  future  research.  The 
following  is  a  list  of  possible  future  project  ideas. 

1.  This  research  demonstrated  the  performance  advantages  of  a  terminal-control  ap¬ 
proach.  However,  inherent  in  this  approach  was  the  assumption  that  the  manipulator 
could  be  well  represented  by  a  linear-time-invariant  state-space  model.  For  increased 
generality,  the  prospect  of  extending  this  approach  to  handle  conflguration  depen¬ 
dency  should  be  investigated. 

2.  This  research  has  illustrated  the  performance  benefits  that  result  from  the  redundancy 
introduced  by  the  mini-manipulator.  In  a  similar  way,  there  may  be  performance  bene¬ 
fits  inherent  in  redundant-link  flexible  manipulators.  There  are  certainly  applications 
where  redundant  manipulators  will  be  required:  weaving  through  space  structures, 
grasping  objects  in  different  configurations,  etc.  As  a  result,  it  will  be  important  to 
understand  how  best  to  exploit  the  redundancy  to  meet  performance  objectives. 

3.  Numerous  projects  have  been  performed  involving  the  use  of  cooperating  manipula¬ 
tors  to  accomplish  a  coordinated  task.  To  date,  this  research  has  involved  the  use  of 
rigid-link  robots  only.  To  investigate  cooperative  manipulation  with  two  flexible-link 
robots,  the  problem  of  two  flexible  manipulators  with  mini-manipulators  cooperatively 
manipulating  an  object  should  be  looked  into.  A  preliminary  step  in  this  investiga¬ 
tion  will  probably  require  force  control  capability  at  the  end  of  a  multi-link  flexible 
manipulator  with  a  mini-manipulator. 

4.  Outside  of  ARL,  there  has  been  a  large  research  eflFort  in  the  development  of  dis¬ 
tributed  actuators  and  sensors  [49].  The  redundant  actuation  and  sensing  introduced 
by  distributed  control  will  enable  the  development  of  fundamentally  new  control  ap¬ 
proaches  for  flexible  systems.  A  large-scale  mock  up  of  a  flexible  beam  with  a  number 
of  actuators  and  sensors  mounted  along  the  link  would  enable  the  development  of  new 
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control  approaches  while  researchers  work  to  improve  the  actual  sensor  and  actuator 
technology.  Possible  approaches  that  might  be  used  include:  the  design  of  indepen¬ 
dent  modal  controllers  in  which  a  group  of  actuators  are  used  to  control  each  mode 
individually;  the  use  of  neural  networks;  the  development  of  a  hybrid  approach  that 
intelligently  merges  neural  networks  and  advanced  flexible-system  control  theory. 

5.  Much  of  the  research  in  flexible  manipulators  has  been  limited  to  2-D.  Although 
this  limitation  has  enabled  some  fundamental  flexibility  issues  to  be  studied  and  well 
understood,  there  may  be  fundamental  issues,  rather  than  just  complexity  issues, 
associated  with  3-D  out  of  plane  vibrations.  Possible  issues  include  coupling  between 
modes,  torsion/bending  coupling,  actuator  selection,  and  sensing. 

6.  In  this  research,  it  was  assumed  that  the  manipulator  base  was  flxed.  However,  for 
many  applications,  this  assumption  may  not  be  valid.  Previous  research  has  addressed 
the  problem  of  controlling  a  rigid  robot  mounted  on  a  compliant  or  mobile  base.  Ex¬ 
tending  this  capability  to  include  link  flexibility  will  increase  the  scope  of  applications 
for  flexible-link  manipulators. 

7.  Recently,  there  have  been  significant  advances  made  in  robust-control  theory.  These 
robust-design  approaches  could  be  incorporated  into  the  sequential  regulator-design 
procedure  described  in  Chapter  5. 
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Appendix  A 


The  State-Space  Model 
Concatenation  Method 


This  appendix  presents  a  summary  of  the  State-Space  Model  Concatenation  Method  [28] 
that  was  utilized  in  the  modular  modelling  approach  presented  in  Chapter  3. 

The  State-Space  Model  Concatenation  Method  takes  as  input  two  subsystem  state-space 
models  of  the  form 


Xj  =  AjXj  -h  B“uj(t)  -f  Bfei 

Zj  =  Cfxi  (A.l) 

ii  =  C{xi 
di  =  Cfxi 

where  the  dimensions  for  subsystem  z  =  1,2,  are  specified  as  the  state  vector  G 
external  input  vector  u(t)^  G  internal  effort  vector  at  the  interface  G 

monitored  output  vector  G  internal  fiow  vector  at  the  interface  G  and 

internal  displacement  vector  at  the  interface  G  with  A^,  B^,  Bf,  Cf,  c(,  and  Cf 

having  consistent  dimensions.  The  output  of  the  algorithm  is  a  state-space  realization  for 
the  total  system. 

Algorithm  Summary 

Given  the  subsystem  state-space  models,  the  total-system  model  is  found  by  performing 
the  following  steps: 
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1 .  Form  the  output  matrices  of  the  form 


^Zcom 


2.  Form  the  F  matrix  defined  by 

F 


c(b^  + 


3.  Form  the  augmented  output  matrix  Cf 


^Zcom 


where  [28]  gives  an  algorithm  for  computing  such  that 


4.  Form  the  total-system  state-space  realization  of  the  form 

X  =  Ax-hBu 
z  =  Cx 


X  =  {  xf  }  , 

u  =  {  uf  uj  ]  , 


Z  =  i  Zt  Zn 


A  and  B  are  most  easily  expressed  in  block  form  as 


All 

Ai2 

Bn 

B12 

A21 

A22 

B21 

B22 

such  that 


An  =  Ai-B^r(c{Ai-C{A2H[cr'") 

Ai2  =  Bfrc{A2H[ 

A21  =  Cf "'^A2Hf  +  Cf "“^Bir  (c{Ai  -  c{A2H[ Cf 

A22  =  C^*"‘^A2Hi’ -  Cr^B^rC^AsHl’ 

Bn  =  B^-B^rc(B^ 

Bi2  -  BfrC^B^ 

B21  =  Cf"‘^B^rc{B^ 

_  {-^zind-r>u  r^zind-rx^TC^i 

22  —  ^2  -^2  “  ^2  ■^2^'^2-*^2 


and  the  output  matrix  is  given  by 


^nzix{ns2—Tic) 

CzuT  r^zcom  C^zxsT 
2t±i  Oj  ^2-^2 


Hi  and  H2  are  partitioned  matrices  of  (Cf)  ^  such  that 


X2 


HiCf”"*xi  +H2z|"‘^. 


(A.6) 

(A.7) 

(A.8) 
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Appendix  B 


Mini-Manipulator  Kinematics 


•  This  appendix  derives  kinematic  relations  used  to  calculate  the  outer-link  angles,  the  Carte¬ 

sian  tip  position  relative  to  the  mini-manipulator  base,  and  the  Jacobian  of  the  mini¬ 
manipulator  [27].  Various  lengths  and  angles  associated  with  the  mini-manipulator  are 
shown  in  Figure  B.l. 


B.l  Kinematic  Constraints 


The  closed-kinematic  chain  configuration  of  the  mini-manipulator  imposes  the  following 
constraint  equations: 


=  LiUm2S2  +  ^^2^^771454 

(B.l) 

L/2Ufn^C^ 

=  LiUm2C2  +  L2UjnAC4 

(B.2) 

where  Li,  L2,  and  qmi  are  defined  in  Figure  B.l, 

Si  ^ 

sin  qmi 

(B.3) 

Q  ^ 

cos  Qmi 

(B.4) 

and 

'^mi 

~  Qmi’ 

(B.5) 

Multiplying  Equation  (B.l)  by  C4,  Equation  (B.2)  by  S4,  adding  the  resulting  equations, 
and  solving  for  UmZ  yields 


— 


L2  S43 


(B.6) 
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Figure  B.l:  Mini-Manipulator  Schematic 

Various  lengths  and  angles  associated  with  the  mini- manipulator  kinematics  are 
shown  in  this  hgure. 


where 

Sij  =  sin  (qmi  -  q-mj)  ■  (B.7) 

Similarly,  multiplying  Equation  (B.l)  by  C3,  Equation  (B.2)  by  S3,  adding  the  resulting 
equations,  and  solving  for  UmA  yields 


Ll  UmlS\z  -  Um2S23 


B.2.  OUTER  JOINT  ANGLES 
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B.2  Outer  Joint  Angles 


Since  only  the  inner  motor  angles  are  measured,  the  outer  angles,  QmZ  and  qmA->  niust  be 
calculated  based  on  kinematic  relationships.  From  the  following  relations: 


d\  —  Lb  +  Li  (sj  —  §2) 

(B.9) 

C?2  =  Li  (C2  -  Cl) 

(B.IO) 

=  C?i  “f*  6^2 

(B.ll) 

,  _i  di 

a  =  tan  — 

(B.12) 

=  90  deg —a 

(B.13) 

LI  -  L2  +  4  -  212^3  cos  {qms  -  a) , 

(B.14) 

expressions  for  qm3  and  Qmi  can  be  determined 

_i  da 

qm3  =  COS  —  -  a 

(B.15) 

qmA  =  180  deg  -2q:  -  qmz- 

(B.16) 

The  following  sines  and  cosines  of  various  angle  combinations  will  be  used  in  later  calcula¬ 


tions: 


where 


d\ “h  ^2 

“  2L2 

(B.17) 

(^2^4  ~  dl 

~  2L2 

(B.18) 

“  2L2 

(B.19) 

^2^4  +  d\ 

~  2L2 

tO 

(B.20) 

^43  = 

(B.21) 

‘■i/iF 

(B.22) 

B.3  Cartesian  Tip  Position 

Knowing  the  outside  motor  angles,  the  tip  position  of  the  mini-manipulator  relative  to  the 
mini-manipulator  reference  frame  is  given  by: 
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did^  +  d2 

=  L1C1  +  L2 — 7N - 

IL2 

=  +  Lisi  +  L2SZ 

Lb  ,  r  ,  r  “  di 

=  -  +  Lm  +  L2^^ 


(B.23) 


(B.24) 


B.4  Mini-Manipulator  Jacobian 

The  Jacobian  relates  the  motor  velocities  of  the  mini- manipulator  to  the  tip  velocity.  It  is 
defined  by  the  equation 


(B.25) 

(B.26) 

^yr  —  L\C\XL'fYi\  -b  Z/2C3U77J3.  (B.27) 

Substituting  Equation  (B.6)  into  this  equation  results  in 

u,.  =  -Li  +  (B.28) 

V  S43  ) 

,  J  S3S24 

+Ll - Um2 

S43 

Uyr  =  Li[c,  +  ^-^^Ural  (B.29) 

T  C3S24 

-Li - Ujn2- 

S43 

Therefore,  by  the  definition  of  the  Jacobian  in  Equation  (B.25),  the  Jacobian  of  the 
mini-manipulator  is 

_  r,  r,  SM24. 

j  ^1.43  .  (B.30) 


Differentiating  Equations  (B.23)  and  (B.24)  with  respect  to  time  produces 

'^xr  ~  LiSiUml  L2S^Ujyi3 


(B.30) 


B.5  Inverse  Kinematics 


The  inverse  kinematics  of  the  mini-manipulator  were  derived  following  the  method  described 
in  [50].  Defining 

pi  ~  QmZ  ""  Qml  (B.31) 

P2  ~  Qm4  ~  9m2i  (B.32) 


p2  —  ^m4  Qm2i 


B.5.  INVERSE  KINEMATICS 


125 


the  relative  mini-manipulator  tip  position  can  be  expressed  as  follows: 


Qxr  —  LiCi  +  L2  COs{qml  +  Pi) 

L  Q 

Qyr  ==  LiSi  L2Sm{qml  +  Pi)  ^ 


Squaring  and  adding  Equations  (B.33)  and  (B,34)  yields 


cos(pi)  = 


qIt  +  {Pyr  -  ~  Li~  Ll 

2L1L2 


Assuming  that  Qxr  ^ind  Qyr  are  within  the  workspace  of  the  mini-manipulator, 
determined  from  the  expression 


where 


Pi 


=  tan 


1  sin(pi) 
cos(pi) 


sin(pi)  =  -y/l  -  cos2(pi). 


Having  solved  for  pi,  qmi  is  given  as 


Qml  — 


—  tan  ^ 


h 

h 


where 


ki  =  Li -t- L2COs(pi) 
k2  -  L2sin(p2). 


(B.33) 

(B.34) 


(B.35) 
can  be 

(B.36) 

(B.37) 

(B.38) 

(B.39) 

(B.40) 


q.ra2  can  be  determined  following  the  same  procedure. 


Appendix  C 

Modelling  the  Flexible  Links 


The  State-Space  Model  Concatenation  Method  requires  models  of  the  flexible  links  in  the 
form 


Xj  =  AjXj  +  B“ui(0  +  B?ei 

zj  =  Cfxj  (C.l) 

=  C/x, 
di  =  Cfx, 


where  the  dimensions  for  subsystem  i,  i  =  1,2,  are  specified  as  the  state  vector  Xj  € 
external  input  vector  u(t)j  €  internal  effort  vector  at  the  interface  Oj  € 

monitored  output  vector  Zj  €  internal  flow  vector  at  the  interface  fj  €  and 

internal  displacement  vector  at  the  interface  di  €  1R"‘^S  with  A^,  B“,  B|,  Cf,  C{,  and  Cf 
having  consistent  dimensions. 

The  approach  used  to  develop  these  subsystem  models  involved  a  four-step  process. 
First,  finite-element  subsystem  models  of  the  flexible  links  were  developed.  Second,  these 
subsystem  models  were  converted  to  modal  form.  Third,  model  reduction  on  the  subsys¬ 
tem  models  was  performed.  Fourth,  the  resulting  reduced-order  subsystem  models  were 
converted  to  the  state-space  form  of  Equation  (C.l).  The  following  sections  detail  the  four 
steps  of  this  process. 
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C.l  Finite-Element  Models 

Using  the  finite-element  modelling  approach,  subsystem  models  of  the  links  were  developed 
of  the  form 

Miqj(f)  +  Kjqj(i)  =  Q“ui  +  Qfe,  (C.2) 

where  Mj  is  the  symmetric  nxn  mass  or  inertia  matrix,  Kj  is  the  symmetric  nxn  stiffness 
matrix,  and  qj(t)  is  an  n-dimensional  vector  of  time-dependent  generalized  coordinates 
which  completely  describe  the  motion  of  the  subsystem.  The  subsystem  output  Zj  and 
displacement  at  the  interface  are  of  the  form 

Zz  =  Tfqi(t)  (C.3) 

d,  =  Tfqiit).  (C.4) 

C.2  Modal  Form 

Converting  the  subsystem  model  in  Equation  (C,2)  to  modal  form  requires  first  solving  the 
generalized  eigenvalue  problem  corresponding  to  the  and  matrices  of  Equation  (C.2) 

KiT-,.  =  j  =  1,  2, . . . ,  n  (C.5) 

where  the  ujij  are  the  natural  frequencies  of  subsystem  z,  and  the  are  the  corresponding 
eigenvectors  which  represent  physically  the  subsystem  natural  modes  or  mode  shapes. 

By  virtue  of  the  expansion  theorem  [37],  any  possible  motion  of  the  system  can  be 
represented  as  a  linear  combination  of  the  system  mode  shapes.  Thus,  at  any  time  the 
vector  of  generalized  coordinates  describing  the  motion  may  be  represented  by  a  linear 
combination  of  the  with  coefficients  (t) 

qi{t)  =  mij  +  772^2  +  ■  ■  ■  +  (C.6) 

In  matrix  form,  Equation  (C.6)  can  be  written  as 

qi{t)  =-  (C.7) 

where  the  columns  of  the  modal  matrix  are  the  system  mode  shapes,  and  mi{t)  is  a 
vector  containing  the  coefficients  For  time-invariant  systems,  the  modal  matrix  is  a 

constant.  As  a  result,  it  immediately  follows  from  Equation  (C.7)  that 


qi(t)  ^  ^^m^(t). 


(C.8) 


C.2.  MODAL  FORM 
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If  the  coefficients  of  Equation  (C.6)  are  defined  as 

mi(t)  =  (C.9) 

and  the  system  mode  shapes  are  orthonormal  and  normalized  such  that 

k,j  =  1,2,... ,n  (C.IO) 

then 

=a;2  j  =  l,2,...,n.  (C.ll) 

Substituting  Equations  (C.7)  and  (C.8)  into  Equation  (C.2)  yields 

=  Q>i  +  (C.12) 

Due  to  the  normalization  in  Equation  (C.IO),  premultiplying  Equation  (C.12)  by  yields 
a  set  of  independent  equations 

mj(t)  +  Uiiriiit)  =  (C.13) 

where  cj?  is  an  n  x  n  matrix  with  diagonal  elements  cuf. ,  and  the  mi-  (t)  are  known  as  normal 
or  modal  coordinates.  The  corresponding  subsystem  outputs  Zj  and  displacements  are 
given  as 


Zi  =  (C.14) 

di  =  Tf^imi(t).  (C.15) 

The  Damping  Matrix  The  experimental  system  contains  inherent  damping.  As  a  re¬ 
sult,  it  is  necessary  to  include  damping  in  the  model  Damping  is  incorporated  by  assuming 
a  damping  ratio  for  each  mode.  The  modal  damping  ratios  are  often  determined  experi¬ 
mentally.  The  modal  damping  matrix  is  given  by 

Cmodai  ^  diag[o  2Cqa;,,  2Ci,uJi,  ...  2Ci„c.i„  ]  (C.16) 

where  are  the  assumed  modal  damping  ratios. 

The  resulting  modal  equations  of  motion  are 

rhi{t)  +  C^°‘'“'rhi(f)  +  ujiriiit)  =  (C.17) 
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C.3  Model  Reduction 

Typically,  the  number  of  generalized  coordinates  in  the  finite-element  model  is  significantly 
greater  than  the  number  of  system  modes  needed  to  accurately  represent  the  system  for 
the  purposes  of  control.  Consequently,  it  is  necessary  to  develop  a  reduced-order  model. 
Model  reduction  is  accomplished  by  simply  truncating  high-frequency  modes  from  the  model 
describe  in  Equation  (C.17). 

C.4  State-Space  Form 

Given  a  model  of  the  form  of  Equation  (C.17)  (or  its  reduced-order  equivalent),  the  state- 
space  model  is  given  by 


where 


Xi 

—  -h 

Zi 

o 

II 

f^ 

=  cfxi 

cl^ 

II 

O 

mi 

Xj 

— 

ihi 

0 

I 

Ai 

1 

j^modal 

0 

Bf 

1 - 

£ 

1 

0 

Bf 

— 

.D 

1 _ 

Zi 

TfTf 

i  0 

di 

TfTfi  0 

fi 

jo  1 

1  • 

(C.18) 


(C.19) 

(C.20) 

(C.21) 

(C.22) 

(C.23) 

(C.24) 

(C.25) 


Appendix  D 

Terminal  Controllers 


This  appendix  gives  a  brief  background  of  other  applications  involving  terminal  controllers. 
Also  included  is  an  example  problem  to  illustrate  the  performance  advantages  introduced  by 
the  time- varying  gains  of  the  terminal  controller  and  the  solution  to  the  terminal  controller 
problem.  This  solution  is  described  in  detail  in  [46]. 


D.l  Background 

Terminal  controllers  have  been  used  in  a  number  of  applications  where  the  controller  re¬ 
quirements  are  similar  to  the  requirements  for  the  rapid  repositioning  of  multi-link  flexible 
manipulators.  The  idea  was  first  introduced  in  [51]  where  time-varying  gains  were  used 
to  obtain  optimal-state  estimates  in  the  presence  of  w'hite  noise.  It  was  also  pointed  out 
in  [51]  that  because  of  the  duality  between  the  optimal-estimation  and  optimal-regulation 
problems,  time-varying  feedback  gains  are  also  the  solution  of  the  optimal-regulation  prob¬ 
lem  with  a  specified  final  time.  Since  then,  terminal  controllers  with  time-varying  gains 
have  been  implemented  to  solve  a  number  of  control  problems  where  terminal  constraints 
are  present.  For  example,  the  guidance  system  of  the  Saturn  launch  vehicle  utilized  time- 
varying  feedback  gains  to  calculate  the  optimum  thrust  direction  for  minimum-propellant 
consumption  to  meet  the  required  end  conditions  for  the  flight  vehicle  [52].  Similarly,  [53] 
discusses  the  application  of  time-varying  gains  to  the  Mars  entry  problem  where  the  goal 
of  the  controller  was  to  minimize  errors  in  a  set  of  pre-specified  terminal  conditions  in  the 
resence  of  atmospheric  density  uncertainties.  [46]  gives  an  example  in  which  time-varying 
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estimator  and  controller  feedback  gains  are  shown  to  be  the  optimal  solution  of  the  missile 
intercept  guidance  problem  with  random  target  acceleration. 

D.2  Performance  Advantages 

The  performance  benefits  of  this  trajectory  generation  approach  stem  from  the  time- varying 
feedback  gains  of  the  terminal  controller.  To  illustrate  the  performance  advantages  of 
terminal  controllers  over  constant-gain  controllers,  consider  the  first-order  system  described 
by 


X  =  —ax  +  u  (D-1) 

where  rr,  a,  and  u  are  scalars,  and  x(to)  =  xo  is  given.  The  goal  is  to  design  a  controller 
which  will  bring  the  state  from  xq  to  a  final  state,  x{tf)  —  Xf^  within  a  specified  final 
time,  tf^  while  using  a  reasonable  amount  of  control.  An  intuitive  approach  to  solving  this 
problem  is  to  formulate  a  performance  index  as  in  Equation  (4.2) 

J  =  Sfxitf)"^  +  (D.2) 

J  Iq 

In  Equation  (D.2),  the  scalars  Qx{t)  and  Qu{i)  have  been  set  to  zero  and  one  respectively. 
Minimizing  Equation  (D,2)  while  at  the  same  time  satisfying  the  constraint  imposed  by 
Equation  (D.l)  results  in  a  time- varying  feedback  gain  Kxvit)^  and  the  control  law 

UTv{t)  —  ~KTv{t)x{t),  (D.3) 

Now,  for  purposes  of  comparison  and  to  isolate  the  effects  of  time- varying  gains,  consider 
the  design  of  a  constant-gain  controller  defined  as: 

UTi{i)  =  ~KTjx{t),  (D.4) 

where  Kti  is  a  constant  feedback  gain.  Substituting  Equation  (D.4)  into  Equation  (D.l) 
results  in  a  first-order,  linear,  homogeneous  differential  equation  in  x 

X  =  ~{a  +  Kti)x,  (D.5) 

which  has  the  solution 

x{t)  -  (D.6) 
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Substituting  Equation  (D.6)  into  Equation  (D.2)  yields 

J  =  [l  -  e“2(“+-^T/)t/l  ^  (Dj) 

•'  2(a  +  Kti)  >•  J 

Setting  the  partial  derivative  of  Equation  (D.7)  with  respect  to  Kti  equal  to  zero,  and  then 

using  an  iterative  scheme  to  solve  for  Kti  will  yield  the  constant  feedback  gain  Kti  which 

minimizes  Equation  (D.2)  while  at  the  same  time  satisfying  the  constraint  in  Equation  (D.l). 

For  the  case  a  =  1,  t/  =  1,  and  5/  =  100,  the  response  of  the  first-order  system  for  both 


Figure  D.l:  TV  Controller  vs.  TI  Controller 

Response  of  the  first-order  system  for  both  time-varying  (TV)  and  time-invariant 
(TI)  feedback  gains.  The  TV  controller  brings  the  state  to  its  desired  terminal  value 
while  the  TI  controller  is  incapable  of  meeting  the  terminal  constraint 


the  time-varying  and  time-invariant  controllers  is  shown  in  Figure  D.l.  From  Figure  D.l  it 
is  evident  that  while  using  less  peak  control  effort,  the  TV  controller  is  able  to  bring  the 
system  to  the  desired  terminal  state,  at  the  desired  final  time,  while  the  TI  controller  is 
incapable  of  meeting  the  terminal  constraints. 

Figure  D.2  is  a  plot  of  the  time-varying  feedback  gain  as  a  function  of  time.  The  gain 
increases  exponentially  as  t  approaches  the  final  time  tf.  As  a  result,  as  the  state  error 
decreases,  the  TV  controller  is  still  able  to  output  a  sufficient  control  to  quickly  bring  the 
state  to  zero.  The  TI  controller,  on  the  other  hand,  outputs  initially  a  large  control  when  the 
state  error  is  largest.  As  the  state  error  decreases,  so  does  the  output  of  the  TI  controller.  As 
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Tune-Varying  Feedback  Gain  vs.  Time 


Time  in  Seconds 


Figure  D.2:  TV  Feedback  Gain  vs.  Time 

This  figure  illustrates  how  the  time-varying  feedback  gain  varies  with  time  for  the 
first-order  system.  Because  the  feedback  gain  increases  as  the  state  error  decreases, 
the  controller  is  capable  of  generating  a  sufRcient  control  signal  to  meet  the  terminal 
constraint. 


a  result,  the  TI  controller  is  only  able  to  bring  the  system  asymptotically  close  to  the  desired 
terminal  state.  Finalty,  for  the  responses  shown  in  Figure  D.2,  the  TV  controller  yields  a 
lower  value  of  J  (J(Ktv)  =  29.9)  than  the  TI  controller  {J(Kti)  =  71.0),  and  therefore, 
the  TV  controller  is  a  better  controller  for  the  performance  index  of  Equation  (D.2). 

Advantages  As  illustrated  by  this  example,  the  time- varying  gains  of  the  terminal  con¬ 
troller  result  in  two  advantages  over  a  constant-gain  controller: 

1.  The  terminal  controller  yields  less  terminal-state  error. 

2.  The  terminal  controller  offers  improved  performance  with  less  peak  control  effort. 

D.3  Terminal- Controller  Solution 

The  terminal  controller  problem  is  to  minimize  a  cost  function  of  the  form: 

J  =  t(x'^S/x)t=t.  +  i  V[x'^Qi(i)x  +  u'^Q„(t)u]di,  (D.8) 

2  ‘  I  Jto 

subject  to  the  linear  constraints  imposed  by 


x(t)  =  Ax(t)  +  Bu(t), 


(D.9) 
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In  Equation  D.8),  Sf  and  Qx{t)  are  positive  semidefinite  matrices  which  weight  the 
values  of  the  states  at  the  terminal  time  x(t/)  and  the  values  of  the  intermediate  states 
x(t),  and  Quit)  is  a  positive  definite  matrix  which  weights  the  control  efifort  u  expended 
during  the  slew.  Equation  (D.8)  is  written  for  the  case  where  the  desired  terminal  state  is 
zero. 

Using  methods  described  in  [46],  we  can  solve  for  the  control  input  u(t)  that  will  mini¬ 
mize  Equation  (4.2)  by  simultaneously  solving  the  linear  constraints  in  Equation  (D.9)  and 
the  following  Euler-Lagrange  equations: 


AT 

an 

dn 


m 

ax’ 


=  0, 


(D.IO) 

(D.ll) 


where 

H  =  ix'^QxX -I- iu'^QiiU -f  A'’^(Ax  4- Bu), 


(D.12) 


X{tf)  = 


(D.13) 


and  A  is  a  vector  of  undetermined  Lagrange  multipliers.  Performing  the  differentiations 
specified  in  Equations  (D.ll)  and  (D.ll)  results  in  the  following  linear  two-point  boundary- 
value  problem: 


where 


X 

A  -BQ-Irt' 

X 

A 

-Q^  -AT 

A 

(D.14) 


Ktf)  =  S/x(t/), 


(D.15) 


and  x(^o)  is  given. 

Problem  Solution 

There  are  a  number  of  methods  by  which  the  two-point  boundary- value  problem  of  Equa¬ 
tions  (D.14)  and  (D.15)  can  be  solved.  The  backward  sweep  method  is  described  here 
because  of  its  direct  application  to  solving  the  discrete-time  terminal  controller  problem 
which  is  useful  for  real-time  implementation. 
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Since  the  two-point  boundary-value  problem  is  linear  and  homogeneous,  X{t)  will  be 
proportional  to  x(to)-  Therefore,  defining 

\{t)  —  S(t)x(t),  (D.16) 

and  substituting  Equation  (D.16)  into  Equation  (D.14)  yields  the  matrix  Riccati  equation 

S  =  -SA- A'^Sd-SBQ-^B’^S-Qi,  (D.17) 

which  is  quadratic  in  S.  Equation  (D.17)  can  be  integrated  backward  from  from  the  specified 
terminal  time  t/  to  the  initial  time  to  to  solve  for  S{t).  Once  S(t)  has  been  found,  the 
continuous  control  law  and  time-varying  feedback  gains  can  be  calculated.  Substituting 
Equation  (D.16)  into  the  result  of  Equation  (D.ll),  the  following  feedback  law  for  terminal 
control  can  be  calculated: 

u(0  =  -K{t)x{t), 

Kit)  =  Q„(f)-iBT(t)S(0, 

where  K(i)  is  the  optimally  determined  time- varying  feedback  gain 

Discrete-Time  Solution 

As  mentioned  in  Section  D.3,  for  real-time  implementation,  it  is  useful  to  formulate  and 
solve  the  terminal  controller  problem  in  the  discrete-time  domain.  Given  the  discretized 
linear  constraint  equations 

x(A;  +  l)  =  ^x{k)  +  Tuik),  (D.20) 

(D.21) 

where  k  —  0,1,..., A  and  and  F  are  the  discrete  system  matrices,  the  discrete- time 
terminal  controller  problem  is  to  minimize  the  performance  index 

J  =  ^-x^iN)SfxiN)  + 

1  ^ 

-Y^x'^ik)Qxd{k)xik)  + 

^  k=l 
.  N-l 

-  u^{k)Qud{k)uik), 

^  k=0 


(D.18) 

(D.19) 


(D.22) 
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subject  to  the  linear  constraints  imposed  by  Equation  (D.21).  Equation  (D.22)  is  written 
for  the  case  where  the  desired  terminal  state  is  zero. 

In  many  applications,  the  value  of  the  intermediate  state  is  not  important,  and  Qxd{f^)  is 
set  equal  to  zero  for  all  k.  This  specification  results  in  a  terminal  controller  which  attempts 
to  bring  the  state  to  zero  in  a  given  number  of  time  steps  N  with  minimum  control  energy 
(or  minimum  integral-square  control  in  the  continuous  case).  Using  a  backward  sweep 
algorithm,  the  solution  when  Qxd{k)  =  0  is  given  in  [54]  as 


ip{k) 

= 

Qud{k)  +  T^S{k  +  l)T] 

(D.23) 

K{k) 

-  i>{k)T^S{k  +  l), 

(D.24) 

S{k) 

=  $^[I-rK(A:)]'^S(A:  +  l)$, 

(D.25) 

where  I  is  the  nxn  identity  matrix.  Since  S/  is  specified  by  the  designer.  Equations  (D.23), 
(D.24),  and  (D.25)  can  be  swept  backward  to  calculate  the  optimal  feedback  gain  C(fc)  at 
each  time  step. 

D.4  Software 

To  compute  the  terminal-controller  solution,  the  following  MATLAB  file  was  developed. 

function  [K1 ,K2]  =  backwardSweep(F,G,Sf ,Qx,Qu,Ts ,Tf ) 

%  Matlab.m  file  for  discrete  smoother-follower  problem  as  a  TPBVP. 

%  Calculates  terminal-controller  time-varying  gains. 

7o 

7o  Assumed  plant  form: 

%  xdot  =  Fx  +  Gu; 

7o 

%  Sample  time  is  Ts; 

7o 

7o  Final  time  is  Tf ; 

% 

7o  Solves  for  terminal  controller  assuming  the  following 
7o  performance  index: 

7o  J=x(N) ’*Sf*x(N)  +  Integral  (0  to  tf)  [x’*Qx*x  +  u’ *Qu(t)  *u]  dt ; 

7o 
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7o  Includes  a  time  weighting  on  Qu(t)  given  by: 
%  if  k  >=  .8*(tf/Ts)  +  (to+Ts)/Ts 
7  rho  =  (k-.8*(tf/Ts)  +  (to+Ts)/Ts) *2; 

7  else 
7  rho  =  1; 

7  end 

7 

7 

7  Written  by  Bill  Ballhaus 
7  12-20-91 

7  Total  number  of  steps 
Nn  =  tf/Ts; 

7  System  size 
[ns ,ns3  =size (F)  ; 

[ns,ni]=size(ga) ; 

7  Conversion  to  discrete  domain 
[phi,ga]=c2d(F,G,Ts) ; 

7  Conversion  of  weighting  matrices 
Qud  =  Qu/Ts; 

Qxd  =  Qx/Ts; 

Sbar  =  Sf/Ts; 


7  Backward  sweep  to  calculate  feedback  gain  at  each  step 
for  k=Nn+(to+Ts)/Ts : -1 : (to+Ts)/Ts , 

7  Compute  time  weighting  on  control 
if  k  >=  .8*  Nn  +  (to+Ts)/Ts 
rho  =  (k-.8*Nn  +(to+Ts)/Ts)~2; 
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else 
rho  =  1; 
end 


%  Gain  calculation 
Shat  =  Sbar  +  Qxd; 

reg_gains=inv (rho*Qud*eye (ni) +ga ’ *Shat*ga) * (ga’ *Shat ) ; 
K1 (k , : ) =reg_gains ( 1 , : ) *phi ; 

K2(k, : )=reg_gains(2, : )*phi; 

Sbar=phi ’ * (eye (ns) -ga*reg_gains) ’ +Shat*phi ; 

end 
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Estimator  Design 


The  control  approach  presented  in  Chapter  5  requires  full  knowledge  of  the  state.  Since  all 
states  of  the  system  cannot  be  measured,  an  estimator  is  needed.  This  appendix  presents 
the  estimator  design  for  the  experimental  two-link  flexible  manipulator  with  the  mini¬ 
manipulator  at  its  tip. 

E.l  Design  Approach 

The  estimator-design  approach  consists  of  an  LQG  design  with  a  nonlinear  output  equation. 
The  design  is  based  on  the  model  of  the  main  arm  developed  in  Chapter  3  for  a  ninety  degree 
elbow  angle.  The  system  is  assumed  to  be  of  the  form 

x(t)  =  Ax(t) -1- Bu(t) -t- Bw(t)  (E-1) 

Ysit)  =  Csx(t)-fv(t)  (E.2) 

where  x(f)  is  the  state  vector,  A  is  the  state  transition  matrix,  B  is  the  input  matrix,  u(t) 
is  the  control  vector,  ys(t)  is  a  vector  of  sensed  outputs,  Cg  is  the  output  matrix,  and  the 
process  noise  w(f)  and  measurement  noise  v(t)  covariances  are  given  by 


E[w(t)] 

=  0 

(E.3) 

E[v(t)] 

=  0 

(E.4) 

E[w(t)w'^(t')] 

=  Ryj6{t-t') 

(E.5) 

E[v(t)vT(t')] 

= 

(E.6) 

E[w(t)vT(i')] 

=  0. 

(E.7) 
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The  sensed  outputs  of  the  experimental  main  arm  are 


ys 


A-tip 

Yiip 

^tip 

0i 

02 


(E.8) 


where  Xup,  Yup,  and  Oup  are  the  position  and  orientation  of  the  vision  target  mounted  at 
the  tip  of  the  flexible  main  arm,  and  Oi  is  the  output  of  the  RVDT  located  at  the  joint. 
The  state  estimate  x  is  given  by 


x(t)  =  Ax(t)  +  Bu(t)  +  L(ys(t)  -  y(t))  (E.9) 


where  y{t)  is  the  estimate  of  the  output  and  L  is  the  estimator-gain  matrix.  The  gain 
matrix  is  a  function  of  the  process  and  measurement  noise  properties.  Consequently, 
and  Rj,  provide  a  design  mechanism  for  specifying  how  much  the  estimates  depend  on  the 
model  and  the  sensor  output. 

For  the  two-link  flexible  manipulator,  the  estimate  of  the  output  is  actually  a  nonlinear 
function  such  that 


y{t)  =  Cs(x(t)). 


(E.IO) 


This  function  is  given  in  [5]. 

E.1.1  Coloring  the  Noise  Spectrum 

The  measurements  corresponding  to  the  end-point  position  and  orientation  provide  redun¬ 
dant  DC  information.  As  a  result,  in  steady  state,  there  is  a  bias  on  the  state  estimate  if 
these  three  signals  do  not  correspond  exactly.  Thus,  a  mechanism  is  needed  to  vary  the 
influence  a  sensor  has  on  the  estimator  as  a  function  of  frequency.  To  do  this,  a  technique 
developed  by  Larry  Alder  was  used  which  requires  coloring  the  noise  spectrum  on  the  sensor 
noise.  [9]  details  the  solution  to  the  colored-noise  estimation  problem. 

E.1.2  Modelling  Sensor  Delay 

The  measurments  provided  by  the  vision  sensor  are  delayed  by  approximately  two  sample 
periods.  The  sensor  delay  can  be  modelled  easily  in  the  discrete  domain  [55].  The  delayed 
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measurements  y delay  are  given  by 


y  delay 

Gtin 


^^delay 


(E.ll) 


and  the  non-delayed  (current)  measurements  are  given  by 


(E.12) 


For  each  sample  period  of  delay,  a  new  set  of  delay  states  is  augmented  to  the  original 
plant  state.  For  the  two  sample  period  delay  associated  with  the  vision  system,  the  resulting 
discrete  system  matrices  are  given  by 


Ydelayik  1) 
y delay {k  -  2) 


(E.13) 


(E.14) 


Bw  0  0 


0  0  1 

Csc^,.  0  0 


(E.15) 


(E.16) 


Using  these  matrices,  the  estimator  is  designed  in  the  discrete  domain  with  no  required 
modifications  to  the  noise  spectra. 


E.2  Estimator  Design 

The  estimator  was  designed  by  varying  R^,  and  R^  until  acceptable  estimator  closed-loop 
roots  were  obtained.  The  noise  covariances  used  were 


R.7/J 


.0001  (N-m) 


.0001  (N-m)^ 


(E.17) 
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R 


V 


.001  m2 
.001  m2 

.0000002  (rad)^ 
.000001  (rad/sec. 
.000001  (rad/sec. 


(E.18) 


In  addition,  the  noise  spectrum  on  the  tip-orientation  sensor  was  colored  with  a  first-order 
filter  with  a  3  Hz  break  frequency  (shown  in  Figure  E.l).  The  closed-loop  roots  corre- 


Figure  E.l;  Coloring  the  Tip-Orientation  Noise  Covariance 

This  figure  shows  how  the  noise  spectrum  on  the  tip-orientation  sensor  was  colored 
to  avoid  a  bias  in  the  state  estimates.  This  filter  is  a  first-order  hlter  with  a  3  Hz 
break  frequency. 


spending  to  these  design  parameters  are  given  in  Figure  E.2. 
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Estimator  Closed-Loop  Roots 
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Figure  E.2:  Estimator  Closed-Loop  Roots 

This  figure  shows  the  estimator  closed-loop  roots.  The  closed-loop  roots  are 
sented  by  the  A’s,  while  the  x’s  represent  the  open-loop  roots. 
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Model  and  Gain  Matrices 


F.l  Main  Arm 

F.1.1  Model 

The  model  of  the  main  arm  is  given  by 


where  the  sensed  outputs  are 


X 

=  Ax  +  Bu 

(F.l) 

=  C5X 

(F.2) 

yc 

=  Ccx 

(F.3) 

and  the  controlled  outputs  are 
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The  state  vector  x  is  defined  as 


^20 

^10 

^21 

Xu 

^22 

^12 

^20 

110 
^21 

111 
^22 

112 


(F.6) 


where  Xij  corresponds  to  the  mode  of  subsystem  i,  and  and  &2f  are  the  states 
corresponding  to  the  rate  filters.  The  system  matrices  are 


A 


Ai 


1000  X 

’  0  0 
0  0 
0  0 
0  0 
0  0 
0  0 
0  0 
0  0 
0  0 
0  0 
0  0 
0  0 
0  0 
0  0 


Ai  A2 

0 

0 

0 

0 

0 

0 

0 

0 

-0.3036 

0 

0 

0 

0 

0 


0 

0 

0 

0 

0 

0 

0 

0 

0 

-0.5540 

0 

0 

0 

0 


0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

-2.0285 

0 

0 

0 


0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

-3.2812 

0 


0  0.0010 
0 
0 
0 
0 
0 
0 
0 
0 
0 
0 
0 


0 


0  0.0701 


(F.7) 


(F.8) 
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0.0010 


0.0010 


0.0010 


0.0010 


-0.0002 


-0.0033 


0.6182 


-0.0027 


-0.0063 
0.7071  -( 


-0.051  0.3620  0.4781  0.5599  -0.6249 


-0.063 


0  1.1137 

0.8056  -0.8056 
0  5.7506 

9.8209  7.5953 

0  8.8939 

11.2331  -9.9268 
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-0.6790 

0 

-0.1733 

0 

0.3145 

0 

0 

0 

0 

0 

0 

0 

0 

0 

-0.6790 

0 

-0.1733 

0 

0.3145 

0 

0 

0 

0 

0 

0 

0 

0 

0 


0 

0.4195 

0 

0.6040 

0 

-0.8520 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0.4195 

0 

0.6040 

0 

-0.8520 

0 

0 

0 

0 

0 

0 

0 

0 


1.1137 

0 

-7.6187 

0 

6.7744 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1.1137 

0 

-7.6187 

0 

6.7744 

0 

0 

0 

0 

0 

0 

0 

0 

0 


T  T 

0  0 

0  0 

0  0 

0  0 

0  0 

0  0 

0  1.1137 

0.8056  -0.8056 

0  5.7506 

9.8209  7.5953 

0  8.8939 

11.2331  -9.9268 

0  0 

0  0 


0 

0.8056 

0 

-7.5953 

0 

9.9268 

0 

0 

0 

0 

0 

0 

0 

0 


(F.ll) 


(F.12) 
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F.l. 2 


Gain  Matrices 


The  corresponding  discrete  gain  matrices  for  60.0  Hz  sampling  are 


-0.0001 

-0.0001 

-0.1397 

-0.0073 

0.0076 

0.0002 

0.0000 

0.0695 

0.1055 

-0.0075 

0.0001 

0.0000 

-0.0076 

0.0013 

-0.0270 

• 

0.0003 

0.0004 

-0.0043 

0.0030 

-0.0005 

0.0001 

0.0003 

0.0004 

0.0005 

0.0065 

0.0004 

-0.0003 

-0.0002 

-0.0003 

-0.0004 

-0.0001 

0.0010 

-0.3781 

-0.0193 

0.0190 

• 

0.0007 

-0.0009 

0.2314 

0.1406 

-0.0152 

0.0031 

0.0164 

0.1610 

0.0274 

-0.1156 

0.0319 

0.0258 

0.0628 

-0.0464 

0.0636 

Ld  = 

-0.0038 

0.0221 

-0.0499 

0.0001 

-0.1379 

• 

0.0251 

-0.0235 

-0.0139 

0.0325 

-0.0421 

0.4525 

-0.0096 

0.4559 

0.0603 

0.1197 

-0.0096 

0.5231 

0.2278 

-0.3435 

-0.5922 

0.0001 

0.0001 

0.0926 

0.0047 

0.0014 

• 

0.0000 

-0.0000 

0.0246 

0.0461 

-0.0037 

0.0002 

0.0013 

-0.0656 

-0.0112 

0.2376 

0.0001 

0.0000 

0.0887 

0.0045 

-0.0002 

0.0001 

-0.0003 

0.0227 

0.0453 

-0.0039 

• 

0.0004 

0.0007 

-0.0459 

-0.0083 

0.1777 

-0.0000 

-0.0001 

0.0024 

0.0003 

0.0146 
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-0.6723 

58.8643 

31.7784 

0 

59.3503 

-86.7588 

-85.5838 

0 

-6.5682 

30.7111 

15.5604 

0 

5.2245 

24.3039 

15.5783 

0 

-1.0358 

5.0660 

-0.6785 

0 

-0.1619 

-1.0293 

0.0130 

0 

0 

0 

0 

0 

0.7727 

0 

1.1630 

0 

2.0618 

0 

0.9176 

0 

-0.3286 

0 

0.0467 

0.8995 

0.0722 

1.4695 

0.1048 

2.8389 

-0.0493 

1.7221 

-0.2352 

0.2615 

(F.15) 


where  Ld  is  the  estimator  gain  matrix  with  the  delay  modelled,  is  the  feedback  gain  on 
the  plant  state,  and  is  the  feedback  gain  on  the  filter  states. 
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F.2  Mini- Manipulator 

F.2.1  Model 

The  equations  of  motion  for  the  mini-manipulator  tip  dynamics  are  given  by 


X3  ^  A3X3+B3U3(t) 

Z3  =  C3X3 


(F.16) 

(F.17) 


where 


(F.18) 


0010 

0001 

0000 

0000 


(F.19) 


(F.20) 


and  rriT  =  0.065  kg. 


F.2. 2  Gain  Matrices 


The  corresponding  controller  is  of  the  form 


(F.21) 


F  =  — KpX  —  K^x 


(F.22) 


where 


F  = 


(F.23) 
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X  = 


X  = 

Kp  and  Kj,  are  the  2  x  2  gain  matrices 


Xmj' 

yrriT 

XrriT 

yrriT 


Kp  = 


K,  = 


91.2871  0 

0  91.2871 

3.4449  0 

0  3.4449 


The  discrete  gain  matrices  for  180.0  Hz  sampling  are 

78.8009  0 

K  = 

[  0  78.8009 

3.2006  0 

K«,  = 

0  3.2006 


(F.24) 

(F.25) 


(F.26) 

(F.27) 


(F.28) 

(F.29) 
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